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"
18#include "BulletSoftBody/btSoftBodySolvers.h"
19#include "btSoftBodyData.h"
20#include "LinearMath/btSerializer.h"
21#include "LinearMath/btImplicitQRSVD.h"
22#include "LinearMath/btAlignedAllocator.h"
23#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
24#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
25#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
26#include "BulletCollision/CollisionShapes/btTriangleShape.h"
27#include <iostream>
28//
29static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
30{
31 int N = leafNodes.size();
32 if (N == 0)
33 {
34 return NULL;
35 }
36 while (N > 1)
37 {
38 btAlignedObjectArray<bool> marked;
39 btAlignedObjectArray<btDbvtNode*> newLeafNodes;
40 btAlignedObjectArray<std::pair<int, int> > childIds;
41 btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
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;
141 pm->m_flags = fMaterial::Default;
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];
150 ZeroInitialize(n);
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();
160 setCollisionQuadrature(3);
161 m_fdbvnt = 0;
162}
163
164btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
165 : m_worldInfo(worldInfo)
166{
167 initDefaults();
168}
169
170void btSoftBody::initDefaults()
171{
172 m_internalType = CO_SOFT_BODY;
173 m_cfg.aeromodel = eAeroModel::V_Point;
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;
187 m_cfg.kSKHR_CL = (btScalar)1;
188 m_cfg.kSSHR_CL = (btScalar)0.5;
189 m_cfg.kSR_SPLT_CL = (btScalar)0.5;
190 m_cfg.kSK_SPLT_CL = (btScalar)0.5;
191 m_cfg.kSS_SPLT_CL = (btScalar)0.5;
192 m_cfg.maxvolume = (btScalar)1;
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;
200 m_cfg.collisions = fCollision::Default;
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);
205 m_pose.m_rot.setIdentity();
206 m_pose.m_scl.setIdentity();
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);
212 m_worldTransform.setIdentity();
213 setSolver(eSolverPresets::Positions);
214
215 /* Collision shape */
217 m_collisionShape = new btSoftBodyCollisionShape(this);
218 m_collisionShape->setMargin(0.25f);
219
220 m_worldTransform.setIdentity();
221
222 m_windVelocity = btVector3(0, 0, 0);
223 m_restLengthScale = btScalar(1.0);
224 m_dampingCoefficient = 1.0;
225 m_sleepingThreshold = .04;
226 m_useSelfCollision = false;
227 m_collisionFlags = 0;
228 m_softSoftCollision = false;
229 m_maxSpeedSquared = 0;
230 m_repulsionStiffness = 0.5;
231 m_gravityFactor = 1;
232 m_cacheBarycenter = false;
233 m_fdbvnt = 0;
234
235 // reduced flag
236 m_reducedModel = false;
237}
238
239//
240btSoftBody::~btSoftBody()
241{
242 //for now, delete the internal shape
243 delete m_collisionShape;
244 int i;
245
246 releaseClusters();
247 for (i = 0; i < m_materials.size(); ++i)
248 btAlignedFree(m_materials[i]);
249 for (i = 0; i < m_joints.size(); ++i)
250 btAlignedFree(m_joints[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//
302btSoftBody::Material* btSoftBody::appendMaterial()
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);
309 m_materials.push_back(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;
323 ZeroInitialize(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;
339 m_notes.push_back(n);
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//
372void btSoftBody::appendNode(const btVector3& x, btScalar m)
373{
374 if (m_nodes.capacity() == m_nodes.size())
375 {
376 pointersToIndices();
377 m_nodes.reserve(m_nodes.size() * 2 + 1);
378 indicesToPointers();
379 }
380 const btScalar margin = getCollisionShape()->getMargin();
381 m_nodes.push_back(Node());
382 Node& n = m_nodes[m_nodes.size() - 1];
383 ZeroInitialize(n);
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 {
399 ZeroInitialize(l);
400 l.m_material = mat ? mat : m_materials[0];
401 }
402 m_links.push_back(l);
403}
404
405//
406void btSoftBody::appendLink(int node0,
407 int node1,
408 Material* mat,
409 bool bcheckexist)
410{
411 appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist);
412}
413
414//
415void btSoftBody::appendLink(Node* node0,
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 {
441 ZeroInitialize(f);
442 f.m_material = mat ? mat : m_materials[0];
443 }
444 m_faces.push_back(f);
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//
472void btSoftBody::appendTetra(int model, Material* mat)
473{
474 Tetra t;
475 if (model >= 0)
476 t = m_tetras[model];
477 else
478 {
479 ZeroInitialize(t);
480 t.m_material = mat ? mat : m_materials[0];
481 }
482 m_tetras.push_back(t);
483}
484
485//
486void btSoftBody::appendTetra(int node0,
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 {
515 if (m_collisionDisabledObjects.findLinearSearch(body) == m_collisionDisabledObjects.size())
516 {
517 m_collisionDisabledObjects.push_back(body);
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;
527 m_anchors.push_back(a);
528}
529
530//
531void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
532{
533 DeformableNodeRigidAnchor c;
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 =
541 m_worldInfo->m_sparsesdf.Evaluate(
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;
554 c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
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
566void btSoftBody::removeAnchor(int node)
567{
568 const btSoftBody::Node& n = m_nodes[node];
569 for (int i = 0; i < m_deformableAnchors.size();)
570 {
571 const DeformableNodeRigidAnchor& c = m_deformableAnchors[i];
572 if (c.m_node == &n)
573 {
574 m_deformableAnchors.removeAtIndex(i);
575 }
576 else
577 {
578 i++;
579 }
580 }
581}
582
583//
584void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
585{
586 DeformableNodeRigidAnchor c;
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 =
593 m_worldInfo->m_sparsesdf.Evaluate(
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;
605 c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
606 btVector3 normal = c.m_cti.m_normal;
607 btVector3 t1 = generateUnitOrthogonalVector(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//
640void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
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;
650 m_joints.push_back(pj);
651}
652
653//
654void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Body body)
655{
656 appendLinearJoint(specs, m_clusters[0], body);
657}
658
659//
660void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, btSoftBody* body)
661{
662 appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]);
663}
664
665//
666void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1)
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;
677 m_joints.push_back(pj);
678}
679
680//
681void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Body body)
682{
683 appendAngularJoint(specs, m_clusters[0], body);
684}
685
686//
687void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, btSoftBody* body)
688{
689 appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]);
690}
691
692//
693void btSoftBody::addForce(const btVector3& force)
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;
730 medium.m_density = m_worldInfo->air_density;
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
744 if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag)
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 }
775 else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
776 {
777 if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
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;
819 medium.m_density = m_worldInfo->air_density;
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
829 if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag)
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 }
870 else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
871 {
872 if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
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//
893void btSoftBody::addVelocity(const btVector3& velocity)
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 */
899void btSoftBody::setVelocity(const btVector3& velocity)
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//
930btScalar btSoftBody::getMass(int node) const
931{
932 return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0);
933}
934
935//
936btScalar btSoftBody::getTotalMass() const
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//
983void btSoftBody::setTotalDensity(btScalar density)
984{
985 setTotalMass(getVolume() * density, true);
986}
987
988//
989void btSoftBody::setVolumeMass(btScalar mass)
990{
991 btAlignedObjectArray<btScalar> ranks;
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//
1019void btSoftBody::setVolumeDensity(btScalar density)
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//
1034btVector3 btSoftBody::getLinearVelocity()
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//
1047void btSoftBody::setLinearVelocity(const btVector3& linVel)
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//
1056void btSoftBody::setAngularVelocity(const btVector3& angVel)
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//
1067btTransform btSoftBody::getRigidTransform()
1068{
1069 btVector3 t = getCenterOfMass();
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//
1093void btSoftBody::transformTo(const btTransform& trs)
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//
1103void btSoftBody::transform(const btTransform& trs)
1104{
1105 const btScalar margin = getCollisionShape()->getMargin();
1106 ATTRIBUTE_ALIGNED16(btDbvtVolume)
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();
1121 updateConstants();
1122}
1123
1124//
1125void btSoftBody::translate(const btVector3& trs)
1126{
1127 btTransform t;
1128 t.setIdentity();
1129 t.setOrigin(trs);
1130 transform(t);
1131}
1132
1133//
1134void btSoftBody::rotate(const btQuaternion& rot)
1135{
1136 btTransform t;
1137 t.setIdentity();
1138 t.setRotation(rot);
1139 transform(t);
1140}
1141
1142//
1143void btSoftBody::scale(const btVector3& scl)
1144{
1145 const btScalar margin = getCollisionShape()->getMargin();
1146 ATTRIBUTE_ALIGNED16(btDbvtVolume)
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();
1159 updateConstants();
1160 initializeDmInverse();
1161}
1162
1163//
1164btScalar btSoftBody::getRestLengthScale()
1165{
1166 return m_restLengthScale;
1167}
1168
1169//
1170void btSoftBody::setRestLengthScale(btScalar restLengthScale)
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
1180 if (getActivationState() == ISLAND_SLEEPING)
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;
1195 m_pose.m_wgh.resize(m_nodes.size());
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();
1207 m_pose.m_pos.resize(m_nodes.size());
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;
1214 m_pose.m_rot.setIdentity();
1215 m_pose.m_scl.setIdentity();
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 }
1228 m_pose.m_aqq = m_pose.m_aqq.inverse();
1229
1230 updateConstants();
1231}
1232
1233void btSoftBody::resetLinkRestLengths()
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//
1244btScalar btSoftBody::getVolume() const
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//
1263int btSoftBody::clusterCount() const
1264{
1265 return (m_clusters.size());
1266}
1267
1268//
1269btVector3 btSoftBody::clusterCom(const Cluster* cluster)
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//
1280btVector3 btSoftBody::clusterCom(int cluster) const
1281{
1282 return (clusterCom(m_clusters[cluster]));
1283}
1284
1285//
1286btVector3 btSoftBody::clusterVelocity(const Cluster* cluster, const btVector3& rpos)
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
1351struct NodeLinks
1352{
1353 btAlignedObjectArray<int> m_links;
1354};
1355
1356//
1357int btSoftBody::generateBendingConstraints(int distance, Material* mat)
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 {
1394 btAlignedObjectArray<NodeLinks> nodeLinks;
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//
1472void btSoftBody::randomizeConstraints()
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
1489void btSoftBody::updateState(const btAlignedObjectArray<btVector3>& q, const btAlignedObjectArray<btVector3>& v)
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//
1505void btSoftBody::releaseCluster(int index)
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//
1515void btSoftBody::releaseClusters()
1516{
1517 while (m_clusters.size() > 0) releaseCluster(0);
1518}
1519
1520//
1521int btSoftBody::generateClusters(int k, int maxiterations)
1522{
1523 int i;
1524 releaseClusters();
1525 m_clusters.resize(btMin(k, m_nodes.size()));
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 */
1535 btAlignedObjectArray<btVector3> centers;
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 */
1590 btAlignedObjectArray<int> cids;
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);
1628 btSwap(m_clusters[0], m_clusters[m_clusters.size() - 1]);
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 {
1644 m_clusters.resize(m_tetras.size());
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 {
1660 m_clusters.resize(m_faces.size());
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 {
1679 initializeClusters();
1680 updateClusters();
1681
1682 //for self-collision
1683 m_clusterConnectivity.resize(m_clusters.size() * m_clusters.size());
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 {
1856 btAlignedObjectArray<int> cnodes;
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();
1924 btAlignedObjectArray<int> ranks;
1925 btAlignedObjectArray<int> todelete;
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())
2044 initializeFaceTree();
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
2054bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
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())
2063 initializeFaceTree();
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//
2074void btSoftBody::setSolver(eSolverPresets::_ preset)
2075{
2076 m_cfg.m_vsequence.clear();
2077 m_cfg.m_psequence.clear();
2078 m_cfg.m_dsequence.clear();
2079 switch (preset)
2080 {
2081 case eSolverPresets::Positions:
2082 m_cfg.m_psequence.push_back(ePSolver::Anchors);
2083 m_cfg.m_psequence.push_back(ePSolver::RContacts);
2084 m_cfg.m_psequence.push_back(ePSolver::SContacts);
2085 m_cfg.m_psequence.push_back(ePSolver::Linear);
2086 break;
2087 case eSolverPresets::Velocities:
2088 m_cfg.m_vsequence.push_back(eVSolver::Linear);
2089
2090 m_cfg.m_psequence.push_back(ePSolver::Anchors);
2091 m_cfg.m_psequence.push_back(ePSolver::RContacts);
2092 m_cfg.m_psequence.push_back(ePSolver::SContacts);
2093
2094 m_cfg.m_dsequence.push_back(ePSolver::Linear);
2095 break;
2096 }
2097}
2098
2099void btSoftBody::predictMotion(btScalar dt)
2100{
2101 int i, ni;
2102
2103 /* Update */
2104 if (m_bUpdateRtCst)
2105 {
2106 m_bUpdateRtCst = false;
2107 updateConstants();
2108 m_fdbvt.clear();
2109 if (m_cfg.collisions & fCollision::VF_SS)
2110 {
2111 initializeFaceTree();
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;
2119 m_sst.radmrg = getCollisionShape()->getMargin();
2120 m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
2121 /* Forces */
2122 addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
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 */
2150 updateClusters();
2151 /* Bounds */
2152 updateBounds();
2153 /* Nodes */
2154 ATTRIBUTE_ALIGNED16(btDbvtVolume)
2155 vol;
2156 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2157 {
2158 Node& n = m_nodes[i];
2159 vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
2160 m_ndbvt.update(n.m_leaf,
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);
2176 m_fdbvt.update(f.m_leaf,
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 */
2199 m_rcontacts.resize(0);
2200 m_scontacts.resize(0);
2201 /* Optimize dbvt's */
2202 m_ndbvt.optimizeIncremental(1);
2203 m_fdbvt.optimizeIncremental(1);
2204 m_cdbvt.optimizeIncremental(1);
2205}
2206
2207//
2208void btSoftBody::solveConstraints()
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;
2227 a.m_c0 = ImpulseMatrix(m_sst.sdt,
2228 a.m_node->m_im,
2229 a.m_body->getInvMass(),
2230 a.m_body->getInvInertiaTensorWorld(),
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//
2319void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
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//
2348void btSoftBody::integrateMotion()
2349{
2350 /* Update */
2351 updateNormals();
2352}
2353
2354//
2355btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom, const btVector3& rayTo, btScalar mxt)
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//
2366void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
2367{
2368 btSoftBody::Face& f = *(btSoftBody::Face*)leaf->data;
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//
2383btScalar btSoftBody::RayFromToCaster::rayFromToTriangle(const btVector3& rayFrom,
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//
2416void btSoftBody::pointersToIndices()
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//
2459void btSoftBody::indicesToPointers(const int* map)
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 {
2525 feature = btSoftBody::eFeature::Face;
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;
2540 feature = btSoftBody::eFeature::Face;
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 {
2567 feature = btSoftBody::eFeature::Tetra;
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
2642void btSoftBody::initializeFaceTree()
2643{
2644 BT_PROFILE("btSoftBody::initializeFaceTree");
2645 m_fdbvt.clear();
2646 // create leaf nodes;
2647 btAlignedObjectArray<btDbvtNode*> leafNodes;
2648 leafNodes.resize(m_faces.size());
2649 for (int i = 0; i < m_faces.size(); ++i)
2650 {
2651 Face& f = m_faces[i];
2652 ATTRIBUTE_ALIGNED16(btDbvtVolume)
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 }
2662 btAlignedObjectArray<btAlignedObjectArray<int> > adj;
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;
2691 m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
2692 updateFaceTree(false, false);
2693 rebuildNodeTree();
2694}
2695
2696//
2697void btSoftBody::rebuildNodeTree()
2698{
2699 m_ndbvt.clear();
2700 btAlignedObjectArray<btDbvtNode*> leafNodes;
2701 leafNodes.resize(m_nodes.size());
2702 for (int i = 0; i < m_nodes.size(); ++i)
2703 {
2704 Node& n = m_nodes[i];
2705 ATTRIBUTE_ALIGNED16(btDbvtVolume)
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 }
2715 btAlignedObjectArray<btAlignedObjectArray<int> > adj;
2716 adj.resize(m_nodes.size());
2717 btAlignedObjectArray<int> old_id;
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//
2735btVector3 btSoftBody::evaluateCom() const
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
2748bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
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 =
2761 m_worldInfo->m_sparsesdf.Evaluate(
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//
2777bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
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 =
2790 m_worldInfo->m_sparsesdf.Evaluate(
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, btVector3& a, btVector3& b, 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 bary.setY((d11 * d20 - d01 * d21) / denom);
2820 bary.setZ((d00 * d21 - d01 * d20) / denom);
2821 bary.setX(btScalar(1) - bary.getY() - bary.getZ());
2822}
2823
2824//
2825bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
2826 Face& f,
2827 btVector3& contact_point,
2828 btVector3& bary,
2829 btScalar margin,
2830 btSoftBody::sCti& cti, bool predict) const
2831{
2832 btVector3 nrm;
2833 const btCollisionShape* shp = colObjWrap->getCollisionShape();
2834 const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2835 // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2836 // but resolve contact at x_n
2837 btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2838 : colObjWrap->getWorldTransform();
2839 btScalar dst;
2840 btGjkEpaSolver2::sResults results;
2841
2842 // #define USE_QUADRATURE 1
2843
2844 // use collision quadrature point
2845#ifdef USE_QUADRATURE
2846 {
2847 dst = SIMD_INFINITY;
2848 btVector3 local_nrm;
2849 for (int q = 0; q < m_quads.size(); ++q)
2850 {
2851 btVector3 p;
2852 if (predict)
2853 p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
2854 else
2855 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
2856 btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
2857 wtr.invXform(p),
2858 shp,
2859 local_nrm,
2860 margin);
2861 if (local_dst < dst)
2862 {
2863 if (local_dst < 0 && predict)
2864 return true;
2865 dst = local_dst;
2866 contact_point = p;
2867 bary = m_quads[q];
2868 nrm = local_nrm;
2869 }
2870 if (!predict)
2871 {
2872 cti.m_colObj = colObjWrap->getCollisionObject();
2873 cti.m_normal = wtr.getBasis() * nrm;
2874 cti.m_offset = dst;
2875 }
2876 }
2877 return (dst < 0);
2878 }
2879#endif
2880
2881 // collision detection using x*
2882 btTransform triangle_transform;
2883 triangle_transform.setIdentity();
2884 triangle_transform.setOrigin(f.m_n[0]->m_q);
2885 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);
2886 btVector3 guess(0, 0, 0);
2887 const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2888 btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
2889 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
2890 if (dst >= 0)
2891 return false;
2892
2893 // Use consistent barycenter to recalculate distance.
2894 if (this->m_cacheBarycenter)
2895 {
2896 if (f.m_pcontact[3] != 0)
2897 {
2898 for (int i = 0; i < 3; ++i)
2899 bary[i] = f.m_pcontact[i];
2900 contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2901 const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2902 btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
2903 cti.m_colObj = colObjWrap->getCollisionObject();
2904 dst = results.distance;
2905 cti.m_normal = results.normal;
2906 cti.m_offset = dst;
2907
2908 //point-convex CD
2909 wtr = colObjWrap->getWorldTransform();
2910 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);
2911 triangle_transform.setOrigin(f.m_n[0]->m_x);
2912 btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
2913
2914 dst = results.distance - csh->getMargin() - margin;
2915 return true;
2916 }
2917 }
2918
2919 // Use triangle-convex CD.
2920 wtr = colObjWrap->getWorldTransform();
2921 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);
2922 triangle_transform.setOrigin(f.m_n[0]->m_x);
2923 btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
2924 contact_point = results.witnesses[0];
2925 getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2926
2927 for (int i = 0; i < 3; ++i)
2928 f.m_pcontact[i] = bary[i];
2929
2930 dst = results.distance - csh->getMargin() - margin;
2931 cti.m_colObj = colObjWrap->getCollisionObject();
2932 cti.m_normal = results.normal;
2933 cti.m_offset = dst;
2934 return true;
2935}
2936
2937void btSoftBody::updateNormals()
2938{
2939 const btVector3 zv(0, 0, 0);
2940 int i, ni;
2941
2942 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2943 {
2944 m_nodes[i].m_n = zv;
2945 }
2946 for (i = 0, ni = m_faces.size(); i < ni; ++i)
2947 {
2948 btSoftBody::Face& f = m_faces[i];
2949 const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,
2950 f.m_n[2]->m_x - f.m_n[0]->m_x);
2951 f.m_normal = n;
2952 f.m_normal.safeNormalize();
2953 f.m_n[0]->m_n += n;
2954 f.m_n[1]->m_n += n;
2955 f.m_n[2]->m_n += n;
2956 }
2957 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2958 {
2959 btScalar len = m_nodes[i].m_n.length();
2960 if (len > SIMD_EPSILON)
2961 m_nodes[i].m_n /= len;
2962 }
2963}
2964
2965//
2966void btSoftBody::updateBounds()
2967{
2968 /*if( m_acceleratedSoftBody )
2969 {
2970 // If we have an accelerated softbody we need to obtain the bounds correctly
2971 // For now (slightly hackily) just have a very large AABB
2972 // TODO: Write get bounds kernel
2973 // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2974 // probably do a test and exchange reasonably efficiently.
2975
2976 m_bounds[0] = btVector3(-1000, -1000, -1000);
2977 m_bounds[1] = btVector3(1000, 1000, 1000);
2978
2979 } else {*/
2980 // if (m_ndbvt.m_root)
2981 // {
2982 // const btVector3& mins = m_ndbvt.m_root->volume.Mins();
2983 // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
2984 // const btScalar csm = getCollisionShape()->getMargin();
2985 // const btVector3 mrg = btVector3(csm,
2986 // csm,
2987 // csm) *
2988 // 1; // ??? to investigate...
2989 // m_bounds[0] = mins - mrg;
2990 // m_bounds[1] = maxs + mrg;
2991 // if (0 != getBroadphaseHandle())
2992 // {
2993 // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
2994 // m_bounds[0],
2995 // m_bounds[1],
2996 // m_worldInfo->m_dispatcher);
2997 // }
2998 // }
2999 // else
3000 // {
3001 // m_bounds[0] =
3002 // m_bounds[1] = btVector3(0, 0, 0);
3003 // }
3004 if (m_nodes.size())
3005 {
3006 btVector3 mins = m_nodes[0].m_x;
3007 btVector3 maxs = m_nodes[0].m_x;
3008 for (int i = 1; i < m_nodes.size(); ++i)
3009 {
3010 for (int d = 0; d < 3; ++d)
3011 {
3012 if (m_nodes[i].m_x[d] > maxs[d])
3013 maxs[d] = m_nodes[i].m_x[d];
3014 if (m_nodes[i].m_x[d] < mins[d])
3015 mins[d] = m_nodes[i].m_x[d];
3016 }
3017 }
3018 const btScalar csm = getCollisionShape()->getMargin();
3019 const btVector3 mrg = btVector3(csm,
3020 csm,
3021 csm);
3022 m_bounds[0] = mins - mrg;
3023 m_bounds[1] = maxs + mrg;
3024 if (0 != getBroadphaseHandle())
3025 {
3026 m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
3027 m_bounds[0],
3028 m_bounds[1],
3029 m_worldInfo->m_dispatcher);
3030 }
3031 }
3032 else
3033 {
3034 m_bounds[0] =
3035 m_bounds[1] = btVector3(0, 0, 0);
3036 }
3037}
3038
3039//
3040void btSoftBody::updatePose()
3041{
3042 if (m_pose.m_bframe)
3043 {
3044 btSoftBody::Pose& pose = m_pose;
3045 const btVector3 com = evaluateCom();
3046 /* Com */
3047 pose.m_com = com;
3048 /* Rotation */
3049 btMatrix3x3 Apq;
3050 const btScalar eps = SIMD_EPSILON;
3051 Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0);
3052 Apq[0].setX(eps);
3053 Apq[1].setY(eps * 2);
3054 Apq[2].setZ(eps * 3);
3055 for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
3056 {
3057 const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com);
3058 const btVector3& b = pose.m_pos[i];
3059 Apq[0] += a.x() * b;
3060 Apq[1] += a.y() * b;
3061 Apq[2] += a.z() * b;
3062 }
3063 btMatrix3x3 r, s;
3064 PolarDecompose(Apq, r, s);
3065 pose.m_rot = r;
3066 pose.m_scl = pose.m_aqq * r.transpose() * Apq;
3067 if (m_cfg.maxvolume > 1)
3068 {
3069 const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(),
3070 1, m_cfg.maxvolume);
3071 pose.m_scl = Mul(pose.m_scl, idet);
3072 }
3073 }
3074}
3075
3076//
3077void btSoftBody::updateArea(bool averageArea)
3078{
3079 int i, ni;
3080
3081 /* Face area */
3082 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3083 {
3084 Face& f = m_faces[i];
3085 f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x);
3086 }
3087
3088 /* Node area */
3089
3090 if (averageArea)
3091 {
3092 btAlignedObjectArray<int> counts;
3093 counts.resize(m_nodes.size(), 0);
3094 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3095 {
3096 m_nodes[i].m_area = 0;
3097 }
3098 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3099 {
3100 btSoftBody::Face& f = m_faces[i];
3101 for (int j = 0; j < 3; ++j)
3102 {
3103 const int index = (int)(f.m_n[j] - &m_nodes[0]);
3104 counts[index]++;
3105 f.m_n[j]->m_area += btFabs(f.m_ra);
3106 }
3107 }
3108 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3109 {
3110 if (counts[i] > 0)
3111 m_nodes[i].m_area /= (btScalar)counts[i];
3112 else
3113 m_nodes[i].m_area = 0;
3114 }
3115 }
3116 else
3117 {
3118 // initialize node area as zero
3119 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3120 {
3121 m_nodes[i].m_area = 0;
3122 }
3123
3124 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3125 {
3126 btSoftBody::Face& f = m_faces[i];
3127
3128 for (int j = 0; j < 3; ++j)
3129 {
3130 f.m_n[j]->m_area += f.m_ra;
3131 }
3132 }
3133
3134 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3135 {
3136 m_nodes[i].m_area *= 0.3333333f;
3137 }
3138 }
3139}
3140
3141void btSoftBody::updateLinkConstants()
3142{
3143 int i, ni;
3144
3145 /* Links */
3146 for (i = 0, ni = m_links.size(); i < ni; ++i)
3147 {
3148 Link& l = m_links[i];
3149 Material& m = *l.m_material;
3150 l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST;
3151 }
3152}
3153
3154void btSoftBody::updateConstants()
3155{
3156 resetLinkRestLengths();
3157 updateLinkConstants();
3158 updateArea();
3159}
3160
3161//
3162void btSoftBody::initializeClusters()
3163{
3164 int i;
3165
3166 for (i = 0; i < m_clusters.size(); ++i)
3167 {
3168 Cluster& c = *m_clusters[i];
3169 c.m_imass = 0;
3170 c.m_masses.resize(c.m_nodes.size());
3171 for (int j = 0; j < c.m_nodes.size(); ++j)
3172 {
3173 if (c.m_nodes[j]->m_im == 0)
3174 {
3175 c.m_containsAnchor = true;
3176 c.m_masses[j] = BT_LARGE_FLOAT;
3177 }
3178 else
3179 {
3180 c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im;
3181 }
3182 c.m_imass += c.m_masses[j];
3183 }
3184 c.m_imass = btScalar(1.) / c.m_imass;
3185 c.m_com = btSoftBody::clusterCom(&c);
3186 c.m_lv = btVector3(0, 0, 0);
3187 c.m_av = btVector3(0, 0, 0);
3188 c.m_leaf = 0;
3189 /* Inertia */
3190 btMatrix3x3& ii = c.m_locii;
3191 ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0);
3192 {
3193 int i, ni;
3194
3195 for (i = 0, ni = c.m_nodes.size(); i < ni; ++i)
3196 {
3197 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
3198 const btVector3 q = k * k;
3199 const btScalar m = c.m_masses[i];
3200 ii[0][0] += m * (q[1] + q[2]);
3201 ii[1][1] += m * (q[0] + q[2]);
3202 ii[2][2] += m * (q[0] + q[1]);
3203 ii[0][1] -= m * k[0] * k[1];
3204 ii[0][2] -= m * k[0] * k[2];
3205 ii[1][2] -= m * k[1] * k[2];
3206 }
3207 }
3208 ii[1][0] = ii[0][1];
3209 ii[2][0] = ii[0][2];
3210 ii[2][1] = ii[1][2];
3211
3212 ii = ii.inverse();
3213
3214 /* Frame */
3215 c.m_framexform.setIdentity();
3216 c.m_framexform.setOrigin(c.m_com);
3217 c.m_framerefs.resize(c.m_nodes.size());
3218 {
3219 int i;
3220 for (i = 0; i < c.m_framerefs.size(); ++i)
3221 {
3222 c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com;
3223 }
3224 }
3225 }
3226}
3227
3228//
3229void btSoftBody::updateClusters()
3230{
3231 BT_PROFILE("UpdateClusters");
3232 int i;
3233
3234 for (i = 0; i < m_clusters.size(); ++i)
3235 {
3236 btSoftBody::Cluster& c = *m_clusters[i];
3237 const int n = c.m_nodes.size();
3238 //const btScalar invn=1/(btScalar)n;
3239 if (n)
3240 {
3241 /* Frame */
3242 const btScalar eps = btScalar(0.0001);
3243 btMatrix3x3 m, r, s;
3244 m[0] = m[1] = m[2] = btVector3(0, 0, 0);
3245 m[0][0] = eps * 1;
3246 m[1][1] = eps * 2;
3247 m[2][2] = eps * 3;
3248 c.m_com = clusterCom(&c);
3249 for (int i = 0; i < c.m_nodes.size(); ++i)
3250 {
3251 const btVector3 a = c.m_nodes[i]->m_x - c.m_com;
3252 const btVector3& b = c.m_framerefs[i];
3253 m[0] += a[0] * b;
3254 m[1] += a[1] * b;
3255 m[2] += a[2] * b;
3256 }
3257 PolarDecompose(m, r, s);
3258 c.m_framexform.setOrigin(c.m_com);
3259 c.m_framexform.setBasis(r);
3260 /* Inertia */
3261#if 1 /* Constant */
3262 c.m_invwi = c.m_framexform.getBasis() * c.m_locii * c.m_framexform.getBasis().transpose();
3263#else
3264#if 0 /* Sphere */
3265 const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
3266 const btVector3 inertia(rk,rk,rk);
3267 const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
3268 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
3269 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
3270
3271 c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
3272#else /* Actual */
3273 c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0);
3274 for (int i = 0; i < n; ++i)
3275 {
3276 const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
3277 const btVector3 q = k * k;
3278 const btScalar m = 1 / c.m_nodes[i]->m_im;
3279 c.m_invwi[0][0] += m * (q[1] + q[2]);
3280 c.m_invwi[1][1] += m * (q[0] + q[2]);
3281 c.m_invwi[2][2] += m * (q[0] + q[1]);
3282 c.m_invwi[0][1] -= m * k[0] * k[1];
3283 c.m_invwi[0][2] -= m * k[0] * k[2];
3284 c.m_invwi[1][2] -= m * k[1] * k[2];
3285 }
3286 c.m_invwi[1][0] = c.m_invwi[0][1];
3287 c.m_invwi[2][0] = c.m_invwi[0][2];
3288 c.m_invwi[2][1] = c.m_invwi[1][2];
3289 c.m_invwi = c.m_invwi.inverse();
3290#endif
3291#endif
3292 /* Velocities */
3293 c.m_lv = btVector3(0, 0, 0);
3294 c.m_av = btVector3(0, 0, 0);
3295 {
3296 int i;
3297
3298 for (i = 0; i < n; ++i)
3299 {
3300 const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i];
3301 c.m_lv += v;
3302 c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v);
3303 }
3304 }
3305 c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping);
3306 c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping);
3307 c.m_vimpulses[0] =
3308 c.m_vimpulses[1] = btVector3(0, 0, 0);
3309 c.m_dimpulses[0] =
3310 c.m_dimpulses[1] = btVector3(0, 0, 0);
3311 c.m_nvimpulses = 0;
3312 c.m_ndimpulses = 0;
3313 /* Matching */
3314 if (c.m_matching > 0)
3315 {
3316 for (int j = 0; j < c.m_nodes.size(); ++j)
3317 {
3318 Node& n = *c.m_nodes[j];
3319 const btVector3 x = c.m_framexform * c.m_framerefs[j];
3320 n.m_x = Lerp(n.m_x, x, c.m_matching);
3321 }
3322 }
3323 /* Dbvt */
3324 if (c.m_collide)
3325 {
3326 btVector3 mi = c.m_nodes[0]->m_x;
3327 btVector3 mx = mi;
3328 for (int j = 1; j < n; ++j)
3329 {
3330 mi.setMin(c.m_nodes[j]->m_x);
3331 mx.setMax(c.m_nodes[j]->m_x);
3332 }
3333 ATTRIBUTE_ALIGNED16(btDbvtVolume)
3334 bounds = btDbvtVolume::FromMM(mi, mx);
3335 if (c.m_leaf)
3336 m_cdbvt.update(c.m_leaf, bounds, c.m_lv * m_sst.sdt * 3, m_sst.radmrg);
3337 else
3338 c.m_leaf = m_cdbvt.insert(bounds, &c);
3339 }
3340 }
3341 }
3342}
3343
3344//
3345void btSoftBody::cleanupClusters()
3346{
3347 for (int i = 0; i < m_joints.size(); ++i)
3348 {
3349 m_joints[i]->Terminate(m_sst.sdt);
3350 if (m_joints[i]->m_delete)
3351 {
3352 btAlignedFree(m_joints[i]);
3353 m_joints.remove(m_joints[i--]);
3354 }
3355 }
3356}
3357
3358//
3359void btSoftBody::prepareClusters(int iterations)
3360{
3361 for (int i = 0; i < m_joints.size(); ++i)
3362 {
3363 m_joints[i]->Prepare(m_sst.sdt, iterations);
3364 }
3365}
3366
3367//
3368void btSoftBody::solveClusters(btScalar sor)
3369{
3370 for (int i = 0, ni = m_joints.size(); i < ni; ++i)
3371 {
3372 m_joints[i]->Solve(m_sst.sdt, sor);
3373 }
3374}
3375
3376//
3377void btSoftBody::applyClusters(bool drift)
3378{
3379 BT_PROFILE("ApplyClusters");
3380 // const btScalar f0=m_sst.sdt;
3381 //const btScalar f1=f0/2;
3382 btAlignedObjectArray<btVector3> deltas;
3383 btAlignedObjectArray<btScalar> weights;
3384 deltas.resize(m_nodes.size(), btVector3(0, 0, 0));
3385 weights.resize(m_nodes.size(), 0);
3386 int i;
3387
3388 if (drift)
3389 {
3390 for (i = 0; i < m_clusters.size(); ++i)
3391 {
3392 Cluster& c = *m_clusters[i];
3393 if (c.m_ndimpulses)
3394 {
3395 c.m_dimpulses[0] /= (btScalar)c.m_ndimpulses;
3396 c.m_dimpulses[1] /= (btScalar)c.m_ndimpulses;
3397 }
3398 }
3399 }
3400
3401 for (i = 0; i < m_clusters.size(); ++i)
3402 {
3403 Cluster& c = *m_clusters[i];
3404 if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses))
3405 {
3406 const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt;
3407 const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt;
3408 for (int j = 0; j < c.m_nodes.size(); ++j)
3409 {
3410 const int idx = int(c.m_nodes[j] - &m_nodes[0]);
3411 const btVector3& x = c.m_nodes[j]->m_x;
3412 const btScalar q = c.m_masses[j];
3413 deltas[idx] += (v + btCross(w, x - c.m_com)) * q;
3414 weights[idx] += q;
3415 }
3416 }
3417 }
3418 for (i = 0; i < deltas.size(); ++i)
3419 {
3420 if (weights[i] > 0)
3421 {
3422 m_nodes[i].m_x += deltas[i] / weights[i];
3423 }
3424 }
3425}
3426
3427//
3428void btSoftBody::dampClusters()
3429{
3430 int i;
3431
3432 for (i = 0; i < m_clusters.size(); ++i)
3433 {
3434 Cluster& c = *m_clusters[i];
3435 if (c.m_ndamping > 0)
3436 {
3437 for (int j = 0; j < c.m_nodes.size(); ++j)
3438 {
3439 Node& n = *c.m_nodes[j];
3440 if (n.m_im > 0)
3441 {
3442 const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com);
3443 if (vx.length2() <= n.m_v.length2())
3444 {
3445 n.m_v += c.m_ndamping * (vx - n.m_v);
3446 }
3447 }
3448 }
3449 }
3450 }
3451}
3452
3453void btSoftBody::setSpringStiffness(btScalar k)
3454{
3455 for (int i = 0; i < m_links.size(); ++i)
3456 {
3457 m_links[i].Feature::m_material->m_kLST = k;
3458 }
3459 m_repulsionStiffness = k;
3460}
3461
3462void btSoftBody::setGravityFactor(btScalar gravFactor)
3463{
3464 m_gravityFactor = gravFactor;
3465}
3466
3467void btSoftBody::setCacheBarycenter(bool cacheBarycenter)
3468{
3469 m_cacheBarycenter = cacheBarycenter;
3470}
3471
3472void btSoftBody::initializeDmInverse()
3473{
3474 btScalar unit_simplex_measure = 1. / 6.;
3475
3476 for (int i = 0; i < m_tetras.size(); ++i)
3477 {
3478 Tetra& t = m_tetras[i];
3479 btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
3480 btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
3481 btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
3482 btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
3483 c1.getY(), c2.getY(), c3.getY(),
3484 c1.getZ(), c2.getZ(), c3.getZ());
3485 t.m_element_measure = Dm.determinant() * unit_simplex_measure;
3486 t.m_Dm_inverse = Dm.inverse();
3487
3488 // calculate the first three columns of P^{-1}
3489 btVector3 a = t.m_n[0]->m_x;
3490 btVector3 b = t.m_n[1]->m_x;
3491 btVector3 c = t.m_n[2]->m_x;
3492 btVector3 d = t.m_n[3]->m_x;
3493
3494 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]);
3495
3496 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];
3497 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];
3498 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];
3499 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];
3500 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];
3501 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];
3502 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];
3503 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];
3504 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];
3505 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];
3506 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];
3507 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];
3508
3509 btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det);
3510 btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det);
3511 btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det);
3512
3513 t.m_P_inv[0] = p1;
3514 t.m_P_inv[1] = p2;
3515 t.m_P_inv[2] = p3;
3516 }
3517}
3518
3519static btScalar Dot4(const btVector4& a, const btVector4& b)
3520{
3521 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
3522}
3523
3524void btSoftBody::updateDeformation()
3525{
3526 btQuaternion q;
3527 for (int i = 0; i < m_tetras.size(); ++i)
3528 {
3529 btSoftBody::Tetra& t = m_tetras[i];
3530 btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
3531 btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
3532 btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
3533 btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
3534 c1.getY(), c2.getY(), c3.getY(),
3535 c1.getZ(), c2.getZ(), c3.getZ());
3536 t.m_F = Ds * t.m_Dm_inverse;
3537
3538 btSoftBody::TetraScratch& s = m_tetraScratches[i];
3539 s.m_F = t.m_F;
3540 s.m_J = t.m_F.determinant();
3541 btMatrix3x3 C = t.m_F.transpose() * t.m_F;
3542 s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
3543 s.m_cofF = t.m_F.adjoint().transpose();
3544
3545 btVector3 a = t.m_n[0]->m_q;
3546 btVector3 b = t.m_n[1]->m_q;
3547 btVector3 c = t.m_n[2]->m_q;
3548 btVector3 d = t.m_n[3]->m_q;
3549 btVector4 q1(a[0], b[0], c[0], d[0]);
3550 btVector4 q2(a[1], b[1], c[1], d[1]);
3551 btVector4 q3(a[2], b[2], c[2], d[2]);
3552 btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]),
3553 Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]),
3554 Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2]));
3555 q.setRotation(btVector3(0, 0, 1), 0);
3556 B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness.
3557 btMatrix3x3 Q(q);
3558 s.m_corotation = Q;
3559 }
3560}
3561
3562void btSoftBody::advanceDeformation()
3563{
3564 updateDeformation();
3565 for (int i = 0; i < m_tetras.size(); ++i)
3566 {
3567 m_tetraScratchesTn[i] = m_tetraScratches[i];
3568 }
3569}
3570//
3571void btSoftBody::Joint::Prepare(btScalar dt, int)
3572{
3573 m_bodies[0].activate();
3574 m_bodies[1].activate();
3575}
3576
3577//
3578void btSoftBody::LJoint::Prepare(btScalar dt, int iterations)
3579{
3580 static const btScalar maxdrift = 4;
3581 Joint::Prepare(dt, iterations);
3582 m_rpos[0] = m_bodies[0].xform() * m_refs[0];
3583 m_rpos[1] = m_bodies[1].xform() * m_refs[1];
3584 m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt;
3585 m_rpos[0] -= m_bodies[0].xform().getOrigin();
3586 m_rpos[1] -= m_bodies[1].xform().getOrigin();
3587 m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0],
3588 m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]);
3589 if (m_split > 0)
3590 {
3591 m_sdrift = m_massmatrix * (m_drift * m_split);
3592 m_drift *= 1 - m_split;
3593 }
3594 m_drift /= (btScalar)iterations;
3595}
3596
3597//
3598void btSoftBody::LJoint::Solve(btScalar dt, btScalar sor)
3599{
3600 const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3601 const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3602 const btVector3 vr = va - vb;
3603 btSoftBody::Impulse impulse;
3604 impulse.m_asVelocity = 1;
3605 impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor;
3606 m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3607 m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3608}
3609
3610//
3611void btSoftBody::LJoint::Terminate(btScalar dt)
3612{
3613 if (m_split > 0)
3614 {
3615 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3616 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3617 }
3618}
3619
3620//
3621void btSoftBody::AJoint::Prepare(btScalar dt, int iterations)
3622{
3623 static const btScalar maxdrift = SIMD_PI / 16;
3624 m_icontrol->Prepare(this);
3625 Joint::Prepare(dt, iterations);
3626 m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0];
3627 m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1];
3628 m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0]));
3629 m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1)));
3630 m_drift *= m_erp / dt;
3631 m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia());
3632 if (m_split > 0)
3633 {
3634 m_sdrift = m_massmatrix * (m_drift * m_split);
3635 m_drift *= 1 - m_split;
3636 }
3637 m_drift /= (btScalar)iterations;
3638}
3639
3640//
3641void btSoftBody::AJoint::Solve(btScalar dt, btScalar sor)
3642{
3643 const btVector3 va = m_bodies[0].angularVelocity();
3644 const btVector3 vb = m_bodies[1].angularVelocity();
3645 const btVector3 vr = va - vb;
3646 const btScalar sp = btDot(vr, m_axis[0]);
3647 const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp);
3648 btSoftBody::Impulse impulse;
3649 impulse.m_asVelocity = 1;
3650 impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor;
3651 m_bodies[0].applyAImpulse(-impulse);
3652 m_bodies[1].applyAImpulse(impulse);
3653}
3654
3655//
3656void btSoftBody::AJoint::Terminate(btScalar dt)
3657{
3658 if (m_split > 0)
3659 {
3660 m_bodies[0].applyDAImpulse(-m_sdrift);
3661 m_bodies[1].applyDAImpulse(m_sdrift);
3662 }
3663}
3664
3665//
3666void btSoftBody::CJoint::Prepare(btScalar dt, int iterations)
3667{
3668 Joint::Prepare(dt, iterations);
3669 const bool dodrift = (m_life == 0);
3670 m_delete = (++m_life) > m_maxlife;
3671 if (dodrift)
3672 {
3673 m_drift = m_drift * m_erp / dt;
3674 if (m_split > 0)
3675 {
3676 m_sdrift = m_massmatrix * (m_drift * m_split);
3677 m_drift *= 1 - m_split;
3678 }
3679 m_drift /= (btScalar)iterations;
3680 }
3681 else
3682 {
3683 m_drift = m_sdrift = btVector3(0, 0, 0);
3684 }
3685}
3686
3687//
3688void btSoftBody::CJoint::Solve(btScalar dt, btScalar sor)
3689{
3690 const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3691 const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3692 const btVector3 vrel = va - vb;
3693 const btScalar rvac = btDot(vrel, m_normal);
3694 btSoftBody::Impulse impulse;
3695 impulse.m_asVelocity = 1;
3696 impulse.m_velocity = m_drift;
3697 if (rvac < 0)
3698 {
3699 const btVector3 iv = m_normal * rvac;
3700 const btVector3 fv = vrel - iv;
3701 impulse.m_velocity += iv + fv * m_friction;
3702 }
3703 impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor;
3704
3705 if (m_bodies[0].m_soft == m_bodies[1].m_soft)
3706 {
3707 if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) &&
3708 (impulse.m_velocity.getZ() == impulse.m_velocity.getZ()))
3709 {
3710 if (impulse.m_asVelocity)
3711 {
3712 if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
3713 {
3714 }
3715 else
3716 {
3717 m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]);
3718 m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]);
3719 }
3720 }
3721 }
3722 }
3723 else
3724 {
3725 m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3726 m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3727 }
3728}
3729
3730//
3731void btSoftBody::CJoint::Terminate(btScalar dt)
3732{
3733 if (m_split > 0)
3734 {
3735 m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3736 m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3737 }
3738}
3739
3740//
3741void btSoftBody::applyForces()
3742{
3743 BT_PROFILE("SoftBody applyForces");
3744 // const btScalar dt = m_sst.sdt;
3745 const btScalar kLF = m_cfg.kLF;
3746 const btScalar kDG = m_cfg.kDG;
3747 const btScalar kPR = m_cfg.kPR;
3748 const btScalar kVC = m_cfg.kVC;
3749 const bool as_lift = kLF > 0;
3750 const bool as_drag = kDG > 0;
3751 const bool as_pressure = kPR != 0;
3752 const bool as_volume = kVC > 0;
3753 const bool as_aero = as_lift ||
3754 as_drag;
3755 //const bool as_vaero = as_aero &&
3756 // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
3757 //const bool as_faero = as_aero &&
3758 // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
3759 const bool use_medium = as_aero;
3760 const bool use_volume = as_pressure ||
3761 as_volume;
3762 btScalar volume = 0;
3763 btScalar ivolumetp = 0;
3764 btScalar dvolumetv = 0;
3765 btSoftBody::sMedium medium;
3766 if (use_volume)
3767 {
3768 volume = getVolume();
3769 ivolumetp = 1 / btFabs(volume) * kPR;
3770 dvolumetv = (m_pose.m_volume - volume) * kVC;
3771 }
3772 /* Per vertex forces */
3773 int i, ni;
3774
3775 for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3776 {
3777 btSoftBody::Node& n = m_nodes[i];
3778 if (n.m_im > 0)
3779 {
3780 if (use_medium)
3781 {
3782 /* Aerodynamics */
3783 addAeroForceToNode(m_windVelocity, i);
3784 }
3785 /* Pressure */
3786 if (as_pressure)
3787 {
3788 n.m_f += n.m_n * (n.m_area * ivolumetp);
3789 }
3790 /* Volume */
3791 if (as_volume)
3792 {
3793 n.m_f += n.m_n * (n.m_area * dvolumetv);
3794 }
3795 }
3796 }
3797
3798 /* Per face forces */
3799 for (i = 0, ni = m_faces.size(); i < ni; ++i)
3800 {
3801 // btSoftBody::Face& f=m_faces[i];
3802
3803 /* Aerodynamics */
3804 addAeroForceToFace(m_windVelocity, i);
3805 }
3806}
3807
3808//
3809void btSoftBody::setMaxStress(btScalar maxStress)
3810{
3811 m_cfg.m_maxStress = maxStress;
3812}
3813
3814//
3815void btSoftBody::interpolateRenderMesh()
3816{
3817 if (m_z.size() > 0)
3818 {
3819 for (int i = 0; i < m_renderNodes.size(); ++i)
3820 {
3821 const Node* p0 = m_renderNodesParents[i][0];
3822 const Node* p1 = m_renderNodesParents[i][1];
3823 const Node* p2 = m_renderNodesParents[i][2];
3824 btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
3825 btVector3 unit_normal = normal.normalized();
3826 RenderNode& n = m_renderNodes[i];
3827 n.m_x.setZero();
3828 for (int j = 0; j < 3; ++j)
3829 {
3830 n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
3831 }
3832 n.m_x += m_z[i] * unit_normal;
3833 }
3834 }
3835 else
3836 {
3837 for (int i = 0; i < m_renderNodes.size(); ++i)
3838 {
3839 RenderNode& n = m_renderNodes[i];
3840 n.m_x.setZero();
3841 for (int j = 0; j < 4; ++j)
3842 {
3843 if (m_renderNodesParents[i].size())
3844 {
3845 n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
3846 }
3847 }
3848 }
3849 }
3850}
3851
3852void btSoftBody::setCollisionQuadrature(int N)
3853{
3854 for (int i = 0; i <= N; ++i)
3855 {
3856 for (int j = 0; i + j <= N; ++j)
3857 {
3858 m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N)));
3859 }
3860 }
3861}
3862
3863//
3864void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
3865{
3866 BT_PROFILE("PSolve_Anchors");
3867 const btScalar kAHR = psb->m_cfg.kAHR * kst;
3868 const btScalar dt = psb->m_sst.sdt;
3869 for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i)
3870 {
3871 const Anchor& a = psb->m_anchors[i];
3872 const btTransform& t = a.m_body->getWorldTransform();
3873 Node& n = *a.m_node;
3874 const btVector3 wa = t * a.m_local;
3875 const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt;
3876 const btVector3 vb = n.m_x - n.m_q;
3877 const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR;
3878 const btVector3 impulse = a.m_c0 * vr * a.m_influence;
3879 n.m_x += impulse * a.m_c2;
3880 a.m_body->applyImpulse(-impulse, a.m_c1);
3881 }
3882}
3883
3884//
3885void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
3886{
3887 BT_PROFILE("PSolve_RContacts");
3888 const btScalar dt = psb->m_sst.sdt;
3889 const btScalar mrg = psb->getCollisionShape()->getMargin();
3890 btMultiBodyJacobianData jacobianData;
3891 for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
3892 {
3893 const RContact& c = psb->m_rcontacts[i];
3894 const sCti& cti = c.m_cti;
3895 if (cti.m_colObj->hasContactResponse())
3896 {
3897 btVector3 va(0, 0, 0);
3898 btRigidBody* rigidCol = 0;
3899 btMultiBodyLinkCollider* multibodyLinkCol = 0;
3900 btScalar* deltaV = NULL;
3901
3902 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
3903 {
3904 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3905 va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0);
3906 }
3907 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
3908 {
3909 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
3910 if (multibodyLinkCol)
3911 {
3912 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
3913 jacobianData.m_jacobians.resize(ndof);
3914 jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
3915 btScalar* jac = &jacobianData.m_jacobians[0];
3916
3917 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);
3918 deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
3919 multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
3920
3921 btScalar vel = 0.0;
3922 for (int j = 0; j < ndof; ++j)
3923 {
3924 vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
3925 }
3926 va = cti.m_normal * vel * dt;
3927 }
3928 }
3929
3930 const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
3931 const btVector3 vr = vb - va;
3932 const btScalar dn = btDot(vr, cti.m_normal);
3933 if (dn <= SIMD_EPSILON)
3934 {
3935 const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
3936 const btVector3 fv = vr - (cti.m_normal * dn);
3937 // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3938 const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst);
3939 c.m_node->m_x -= impulse * c.m_c2;
3940
3941 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
3942 {
3943 if (rigidCol)
3944 rigidCol->applyImpulse(impulse, c.m_c1);
3945 }
3946 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
3947 {
3948 if (multibodyLinkCol)
3949 {
3950 double multiplier = 0.5;
3951 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
3952 }
3953 }
3954 }
3955 }
3956 }
3957}
3958
3959//
3960void btSoftBody::PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti)
3961{
3962 BT_PROFILE("PSolve_SContacts");
3963
3964 for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i)
3965 {
3966 const SContact& c = psb->m_scontacts[i];
3967 const btVector3& nr = c.m_normal;
3968 Node& n = *c.m_node;
3969 Face& f = *c.m_face;
3970 const btVector3 p = BaryEval(f.m_n[0]->m_x,
3971 f.m_n[1]->m_x,
3972 f.m_n[2]->m_x,
3973 c.m_weights);
3974 const btVector3 q = BaryEval(f.m_n[0]->m_q,
3975 f.m_n[1]->m_q,
3976 f.m_n[2]->m_q,
3977 c.m_weights);
3978 const btVector3 vr = (n.m_x - n.m_q) - (p - q);
3979 btVector3 corr(0, 0, 0);
3980 btScalar dot = btDot(vr, nr);
3981 if (dot < 0)
3982 {
3983 const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p));
3984 corr += c.m_normal * j;
3985 }
3986 corr -= ProjectOnPlane(vr, nr) * c.m_friction;
3987 n.m_x += corr * c.m_cfm[0];
3988 f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x());
3989 f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y());
3990 f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z());
3991 }
3992}
3993
3994//
3995void btSoftBody::PSolve_Links(btSoftBody* psb, btScalar kst, btScalar ti)
3996{
3997 BT_PROFILE("PSolve_Links");
3998 for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
3999 {
4000 Link& l = psb->m_links[i];
4001 if (l.m_c0 > 0)
4002 {
4003 Node& a = *l.m_n[0];
4004 Node& b = *l.m_n[1];
4005 const btVector3 del = b.m_x - a.m_x;
4006 const btScalar len = del.length2();
4007 if (l.m_c1 + len > SIMD_EPSILON)
4008 {
4009 const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst;
4010 a.m_x -= del * (k * a.m_im);
4011 b.m_x += del * (k * b.m_im);
4012 }
4013 }
4014 }
4015}
4016
4017//
4018void btSoftBody::VSolve_Links(btSoftBody* psb, btScalar kst)
4019{
4020 BT_PROFILE("VSolve_Links");
4021 for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
4022 {
4023 Link& l = psb->m_links[i];
4024 Node** n = l.m_n;
4025 const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst;
4026 n[0]->m_v += l.m_c3 * (j * n[0]->m_im);
4027 n[1]->m_v -= l.m_c3 * (j * n[1]->m_im);
4028 }
4029}
4030
4031//
4032btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
4033{
4034 switch (solver)
4035 {
4036 case ePSolver::Anchors:
4037 return (&btSoftBody::PSolve_Anchors);
4038 case ePSolver::Linear:
4039 return (&btSoftBody::PSolve_Links);
4040 case ePSolver::RContacts:
4041 return (&btSoftBody::PSolve_RContacts);
4042 case ePSolver::SContacts:
4043 return (&btSoftBody::PSolve_SContacts);
4044 default:
4045 {
4046 }
4047 }
4048 return (0);
4049}
4050
4051//
4052btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
4053{
4054 switch (solver)
4055 {
4056 case eVSolver::Linear:
4057 return (&btSoftBody::VSolve_Links);
4058 default:
4059 {
4060 }
4061 }
4062 return (0);
4063}
4064
4065void btSoftBody::setSelfCollision(bool useSelfCollision)
4066{
4067 m_useSelfCollision = useSelfCollision;
4068}
4069
4070bool btSoftBody::useSelfCollision()
4071{
4072 return m_useSelfCollision;
4073}
4074
4075//
4076void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
4077{
4078 switch (m_cfg.collisions & fCollision::RVSmask)
4079 {
4080 case fCollision::SDF_RS:
4081 {
4082 btSoftColliders::CollideSDF_RS docollide;
4083 btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
4084 btTransform wtr = pcoWrap->getWorldTransform();
4085
4086 const btTransform ctr = pcoWrap->getWorldTransform();
4087 const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
4088 const btScalar basemargin = getCollisionShape()->getMargin();
4089 btVector3 mins;
4090 btVector3 maxs;
4091 ATTRIBUTE_ALIGNED16(btDbvtVolume)
4092 volume;
4093 pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
4094 mins,
4095 maxs);
4096 volume = btDbvtVolume::FromMM(mins, maxs);
4097 volume.Expand(btVector3(basemargin, basemargin, basemargin));
4098 docollide.psb = this;
4099 docollide.m_colObj1Wrap = pcoWrap;
4100 docollide.m_rigidBody = prb1;
4101
4102 docollide.dynmargin = basemargin + timemargin;
4103 docollide.stamargin = basemargin;
4104 m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
4105 }
4106 break;
4107 case fCollision::CL_RS:
4108 {
4109 btSoftColliders::CollideCL_RS collider;
4110 collider.ProcessColObj(this, pcoWrap);
4111 }
4112 break;
4113 case fCollision::SDF_RD:
4114 {
4115 btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
4116 if (this->isActive())
4117 {
4118 const btTransform wtr = pcoWrap->getWorldTransform();
4119 const btScalar timemargin = 0;
4120 const btScalar basemargin = getCollisionShape()->getMargin();
4121 btVector3 mins;
4122 btVector3 maxs;
4123 ATTRIBUTE_ALIGNED16(btDbvtVolume)
4124 volume;
4125 pcoWrap->getCollisionShape()->getAabb(wtr,
4126 mins,
4127 maxs);
4128 volume = btDbvtVolume::FromMM(mins, maxs);
4129 volume.Expand(btVector3(basemargin, basemargin, basemargin));
4130 if (m_cfg.collisions & fCollision::SDF_RDN)
4131 {
4132 btSoftColliders::CollideSDF_RD docollideNode;
4133 docollideNode.psb = this;
4134 docollideNode.m_colObj1Wrap = pcoWrap;
4135 docollideNode.m_rigidBody = prb1;
4136 docollideNode.dynmargin = basemargin + timemargin;
4137 docollideNode.stamargin = basemargin;
4138 m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
4139 }
4140
4141 if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
4142 {
4143 btSoftColliders::CollideSDF_RDF docollideFace;
4144 docollideFace.psb = this;
4145 docollideFace.m_colObj1Wrap = pcoWrap;
4146 docollideFace.m_rigidBody = prb1;
4147 docollideFace.dynmargin = basemargin + timemargin;
4148 docollideFace.stamargin = basemargin;
4149 m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
4150 }
4151 }
4152 }
4153 break;
4154 }
4155}
4156
4157//
4158void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
4159{
4160 BT_PROFILE("Deformable Collision");
4161 const int cf = m_cfg.collisions & psb->m_cfg.collisions;
4162 switch (cf & fCollision::SVSmask)
4163 {
4164 case fCollision::CL_SS:
4165 {
4166 //support self-collision if CL_SELF flag set
4167 if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF)
4168 {
4169 btSoftColliders::CollideCL_SS docollide;
4170 docollide.ProcessSoftSoft(this, psb);
4171 }
4172 }
4173 break;
4174 case fCollision::VF_SS:
4175 {
4176 //only self-collision for Cluster, not Vertex-Face yet
4177 if (this != psb)
4178 {
4179 btSoftColliders::CollideVF_SS docollide;
4180 /* common */
4181 docollide.mrg = getCollisionShape()->getMargin() +
4182 psb->getCollisionShape()->getMargin();
4183 /* psb0 nodes vs psb1 faces */
4184 docollide.psb[0] = this;
4185 docollide.psb[1] = psb;
4186 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4187 docollide.psb[1]->m_fdbvt.m_root,
4188 docollide);
4189 /* psb1 nodes vs psb0 faces */
4190 docollide.psb[0] = psb;
4191 docollide.psb[1] = this;
4192 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4193 docollide.psb[1]->m_fdbvt.m_root,
4194 docollide);
4195 }
4196 }
4197 break;
4198 case fCollision::VF_DD:
4199 {
4200 if (!psb->m_softSoftCollision)
4201 return;
4202 if (psb->isActive() || this->isActive())
4203 {
4204 if (this != psb)
4205 {
4206 btSoftColliders::CollideVF_DD docollide;
4207 /* common */
4208 docollide.mrg = getCollisionShape()->getMargin() +
4209 psb->getCollisionShape()->getMargin();
4210 /* psb0 nodes vs psb1 faces */
4211 if (psb->m_tetras.size() > 0)
4212 docollide.useFaceNormal = true;
4213 else
4214 docollide.useFaceNormal = false;
4215 docollide.psb[0] = this;
4216 docollide.psb[1] = psb;
4217 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4218 docollide.psb[1]->m_fdbvt.m_root,
4219 docollide);
4220
4221 /* psb1 nodes vs psb0 faces */
4222 if (this->m_tetras.size() > 0)
4223 docollide.useFaceNormal = true;
4224 else
4225 docollide.useFaceNormal = false;
4226 docollide.psb[0] = psb;
4227 docollide.psb[1] = this;
4228 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4229 docollide.psb[1]->m_fdbvt.m_root,
4230 docollide);
4231 }
4232 else
4233 {
4234 if (psb->useSelfCollision())
4235 {
4236 btSoftColliders::CollideFF_DD docollide;
4237 docollide.mrg = 2 * getCollisionShape()->getMargin();
4238 docollide.psb[0] = this;
4239 docollide.psb[1] = psb;
4240 if (this->m_tetras.size() > 0)
4241 docollide.useFaceNormal = true;
4242 else
4243 docollide.useFaceNormal = false;
4244 /* psb0 faces vs psb0 faces */
4245 calculateNormalCone(this->m_fdbvnt);
4246 this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
4247 }
4248 }
4249 }
4250 }
4251 break;
4252 default:
4253 {
4254 }
4255 }
4256}
4257
4258void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
4259{
4260 if (psb->isActive() || this->isActive())
4261 {
4262 if (this != psb)
4263 {
4264 btSoftColliders::CollideCCD docollide;
4265 /* common */
4266 docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin
4267 docollide.dt = psb->m_sst.sdt;
4268 /* psb0 nodes vs psb1 faces */
4269 if (psb->m_tetras.size() > 0)
4270 docollide.useFaceNormal = true;
4271 else
4272 docollide.useFaceNormal = false;
4273 docollide.psb[0] = this;
4274 docollide.psb[1] = psb;
4275 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4276 docollide.psb[1]->m_fdbvt.m_root,
4277 docollide);
4278 /* psb1 nodes vs psb0 faces */
4279 if (this->m_tetras.size() > 0)
4280 docollide.useFaceNormal = true;
4281 else
4282 docollide.useFaceNormal = false;
4283 docollide.psb[0] = psb;
4284 docollide.psb[1] = this;
4285 docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
4286 docollide.psb[1]->m_fdbvt.m_root,
4287 docollide);
4288 }
4289 else
4290 {
4291 if (psb->useSelfCollision())
4292 {
4293 btSoftColliders::CollideCCD docollide;
4294 docollide.mrg = SAFE_EPSILON;
4295 docollide.psb[0] = this;
4296 docollide.psb[1] = psb;
4297 docollide.dt = psb->m_sst.sdt;
4298 if (this->m_tetras.size() > 0)
4299 docollide.useFaceNormal = true;
4300 else
4301 docollide.useFaceNormal = false;
4302 /* psb0 faces vs psb0 faces */
4303 calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope
4304 this->m_fdbvt.selfCollideT(m_fdbvnt, docollide);
4305 }
4306 }
4307 }
4308}
4309
4310void btSoftBody::setWindVelocity(const btVector3& velocity)
4311{
4312 m_windVelocity = velocity;
4313}
4314
4315const btVector3& btSoftBody::getWindVelocity()
4316{
4317 return m_windVelocity;
4318}
4319
4320int btSoftBody::calculateSerializeBufferSize() const
4321{
4322 int sz = sizeof(btSoftBodyData);
4323 return sz;
4324}
4325
4327const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
4328{
4329 btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer;
4330
4331 btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
4332
4333 btHashMap<btHashPtr, int> m_nodeIndexMap;
4334
4335 sbd->m_numMaterials = m_materials.size();
4336 sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0;
4337
4338 if (sbd->m_materials)
4339 {
4340 int sz = sizeof(SoftBodyMaterialData*);
4341 int numElem = sbd->m_numMaterials;
4342 btChunk* chunk = serializer->allocate(sz, numElem);
4343 //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
4344 SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
4345 for (int i = 0; i < numElem; i++, memPtr++)
4346 {
4347 btSoftBody::Material* mat = m_materials[i];
4348 *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
4349 if (!serializer->findPointer(mat))
4350 {
4351 //serialize it here
4352 btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1);
4353 SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
4354 memPtr->m_flags = mat->m_flags;
4355 memPtr->m_angularStiffness = mat->m_kAST;
4356 memPtr->m_linearStiffness = mat->m_kLST;
4357 memPtr->m_volumeStiffness = mat->m_kVST;
4358 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat);
4359 }
4360 }
4361 serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials);
4362 }
4363
4364 sbd->m_numNodes = m_nodes.size();
4365 sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0;
4366 if (sbd->m_nodes)
4367 {
4368 int sz = sizeof(SoftBodyNodeData);
4369 int numElem = sbd->m_numNodes;
4370 btChunk* chunk = serializer->allocate(sz, numElem);
4371 SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
4372 for (int i = 0; i < numElem; i++, memPtr++)
4373 {
4374 m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce);
4375 memPtr->m_area = m_nodes[i].m_area;
4376 memPtr->m_attach = m_nodes[i].m_battach;
4377 memPtr->m_inverseMass = m_nodes[i].m_im;
4378 memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0;
4379 m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
4380 m_nodes[i].m_x.serializeFloat(memPtr->m_position);
4381 m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
4382 m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
4383 m_nodeIndexMap.insert(&m_nodes[i], i);
4384 }
4385 serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes);
4386 }
4387
4388 sbd->m_numLinks = m_links.size();
4389 sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0;
4390 if (sbd->m_links)
4391 {
4392 int sz = sizeof(SoftBodyLinkData);
4393 int numElem = sbd->m_numLinks;
4394 btChunk* chunk = serializer->allocate(sz, numElem);
4395 SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
4396 for (int i = 0; i < numElem; i++, memPtr++)
4397 {
4398 memPtr->m_bbending = m_links[i].m_bbending;
4399 memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0;
4400 memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1;
4401 memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1;
4402 btAssert(memPtr->m_nodeIndices[0] < m_nodes.size());
4403 btAssert(memPtr->m_nodeIndices[1] < m_nodes.size());
4404 memPtr->m_restLength = m_links[i].m_rl;
4405 }
4406 serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]);
4407 }
4408
4409 sbd->m_numFaces = m_faces.size();
4410 sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0;
4411 if (sbd->m_faces)
4412 {
4413 int sz = sizeof(SoftBodyFaceData);
4414 int numElem = sbd->m_numFaces;
4415 btChunk* chunk = serializer->allocate(sz, numElem);
4416 SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
4417 for (int i = 0; i < numElem; i++, memPtr++)
4418 {
4419 memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0;
4420 m_faces[i].m_normal.serializeFloat(memPtr->m_normal);
4421 for (int j = 0; j < 3; j++)
4422 {
4423 memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1;
4424 }
4425 memPtr->m_restArea = m_faces[i].m_ra;
4426 }
4427 serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]);
4428 }
4429
4430 sbd->m_numTetrahedra = m_tetras.size();
4431 sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0;
4432 if (sbd->m_tetrahedra)
4433 {
4434 int sz = sizeof(SoftBodyTetraData);
4435 int numElem = sbd->m_numTetrahedra;
4436 btChunk* chunk = serializer->allocate(sz, numElem);
4437 SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
4438 for (int i = 0; i < numElem; i++, memPtr++)
4439 {
4440 for (int j = 0; j < 4; j++)
4441 {
4442 m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
4443 memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
4444 }
4445 memPtr->m_c1 = m_tetras[i].m_c1;
4446 memPtr->m_c2 = m_tetras[i].m_c2;
4447 memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0;
4448 memPtr->m_restVolume = m_tetras[i].m_rv;
4449 }
4450 serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]);
4451 }
4452
4453 sbd->m_numAnchors = m_anchors.size();
4454 sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0;
4455 if (sbd->m_anchors)
4456 {
4457 int sz = sizeof(SoftRigidAnchorData);
4458 int numElem = sbd->m_numAnchors;
4459 btChunk* chunk = serializer->allocate(sz, numElem);
4460 SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
4461 for (int i = 0; i < numElem; i++, memPtr++)
4462 {
4463 m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
4464 m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
4465 memPtr->m_c2 = m_anchors[i].m_c2;
4466 m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
4467 memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1;
4468
4469 memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0;
4470 btAssert(memPtr->m_nodeIndex < m_nodes.size());
4471 }
4472 serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]);
4473 }
4474
4475 sbd->m_config.m_dynamicFriction = m_cfg.kDF;
4476 sbd->m_config.m_baumgarte = m_cfg.kVCF;
4477 sbd->m_config.m_pressure = m_cfg.kPR;
4478 sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
4479 sbd->m_config.m_lift = m_cfg.kLF;
4480 sbd->m_config.m_drag = m_cfg.kDG;
4481 sbd->m_config.m_positionIterations = m_cfg.piterations;
4482 sbd->m_config.m_driftIterations = m_cfg.diterations;
4483 sbd->m_config.m_clusterIterations = m_cfg.citerations;
4484 sbd->m_config.m_velocityIterations = m_cfg.viterations;
4485 sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4486 sbd->m_config.m_damping = m_cfg.kDP;
4487 sbd->m_config.m_poseMatch = m_cfg.kMT;
4488 sbd->m_config.m_collisionFlags = m_cfg.collisions;
4489 sbd->m_config.m_volume = m_cfg.kVC;
4490 sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
4491 sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
4492 sbd->m_config.m_softContactHardness = m_cfg.kSHR;
4493 sbd->m_config.m_anchorHardness = m_cfg.kAHR;
4494 sbd->m_config.m_timeScale = m_cfg.timescale;
4495 sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4496 sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
4497 sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
4498 sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
4499 sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
4500 sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
4501 sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
4502
4503 //pose for shape matching
4504 {
4505 sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
4506
4507 int sz = sizeof(SoftBodyPoseData);
4508 btChunk* chunk = serializer->allocate(sz, 1);
4509 SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
4510
4511 m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
4512 memPtr->m_bframe = m_pose.m_bframe;
4513 memPtr->m_bvolume = m_pose.m_bvolume;
4514 m_pose.m_com.serializeFloat(memPtr->m_com);
4515
4516 memPtr->m_numPositions = m_pose.m_pos.size();
4517 memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0;
4518 if (memPtr->m_numPositions)
4519 {
4520 int numElem = memPtr->m_numPositions;
4521 int sz = sizeof(btVector3Data);
4522 btChunk* chunk = serializer->allocate(sz, numElem);
4523 btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
4524 for (int i = 0; i < numElem; i++, memPtr++)
4525 {
4526 m_pose.m_pos[i].serializeFloat(*memPtr);
4527 }
4528 serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]);
4529 }
4530 memPtr->m_restVolume = m_pose.m_volume;
4531 m_pose.m_rot.serializeFloat(memPtr->m_rot);
4532 m_pose.m_scl.serializeFloat(memPtr->m_scale);
4533
4534 memPtr->m_numWeigts = m_pose.m_wgh.size();
4535 memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0;
4536 if (memPtr->m_numWeigts)
4537 {
4538 int numElem = memPtr->m_numWeigts;
4539 int sz = sizeof(float);
4540 btChunk* chunk = serializer->allocate(sz, numElem);
4541 float* memPtr = (float*)chunk->m_oldPtr;
4542 for (int i = 0; i < numElem; i++, memPtr++)
4543 {
4544 *memPtr = m_pose.m_wgh[i];
4545 }
4546 serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]);
4547 }
4548
4549 serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose);
4550 }
4551
4552 //clusters for convex-cluster collision detection
4553
4554 sbd->m_numClusters = m_clusters.size();
4555 sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0;
4556 if (sbd->m_numClusters)
4557 {
4558 int numElem = sbd->m_numClusters;
4559 int sz = sizeof(SoftBodyClusterData);
4560 btChunk* chunk = serializer->allocate(sz, numElem);
4561 SoftBodyClusterData* memPtr = (SoftBodyClusterData*)chunk->m_oldPtr;
4562 for (int i = 0; i < numElem; i++, memPtr++)
4563 {
4564 memPtr->m_adamping = m_clusters[i]->m_adamping;
4565 m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
4566 memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
4567 memPtr->m_collide = m_clusters[i]->m_collide;
4568 m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
4569 memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
4570 m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
4571 m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
4572 m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
4573 memPtr->m_idmass = m_clusters[i]->m_idmass;
4574 memPtr->m_imass = m_clusters[i]->m_imass;
4575 m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
4576 memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4577 m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
4578 m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
4579 memPtr->m_matching = m_clusters[i]->m_matching;
4580 memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
4581 memPtr->m_ndamping = m_clusters[i]->m_ndamping;
4582 memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4583 memPtr->m_adamping = m_clusters[i]->m_adamping;
4584 memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
4585
4586 memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
4587 memPtr->m_numMasses = m_clusters[i]->m_masses.size();
4588 memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
4589
4590 memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
4591 m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
4592 m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
4593 memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
4594
4595 memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
4596 if (memPtr->m_framerefs)
4597 {
4598 int numElem = memPtr->m_numFrameRefs;
4599 int sz = sizeof(btVector3FloatData);
4600 btChunk* chunk = serializer->allocate(sz, numElem);
4601 btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
4602 for (int j = 0; j < numElem; j++, memPtr++)
4603 {
4604 m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
4605 }
4606 serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]);
4607 }
4608
4609 memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0;
4610 if (memPtr->m_masses)
4611 {
4612 int numElem = memPtr->m_numMasses;
4613 int sz = sizeof(float);
4614 btChunk* chunk = serializer->allocate(sz, numElem);
4615 float* memPtr = (float*)chunk->m_oldPtr;
4616 for (int j = 0; j < numElem; j++, memPtr++)
4617 {
4618 *memPtr = m_clusters[i]->m_masses[j];
4619 }
4620 serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]);
4621 }
4622
4623 memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0;
4624 if (memPtr->m_nodeIndices)
4625 {
4626 int numElem = memPtr->m_numMasses;
4627 int sz = sizeof(int);
4628 btChunk* chunk = serializer->allocate(sz, numElem);
4629 int* memPtr = (int*)chunk->m_oldPtr;
4630 for (int j = 0; j < numElem; j++, memPtr++)
4631 {
4632 int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
4633 btAssert(indexPtr);
4634 *memPtr = *indexPtr;
4635 }
4636 serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes);
4637 }
4638 }
4639 serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]);
4640 }
4641
4642 sbd->m_numJoints = m_joints.size();
4643 sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0;
4644
4645 if (sbd->m_joints)
4646 {
4647 int sz = sizeof(btSoftBodyJointData);
4648 int numElem = m_joints.size();
4649 btChunk* chunk = serializer->allocate(sz, numElem);
4650 btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
4651
4652 for (int i = 0; i < numElem; i++, memPtr++)
4653 {
4654 memPtr->m_jointType = (int)m_joints[i]->Type();
4655 m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
4656 m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
4657 memPtr->m_cfm = m_joints[i]->m_cfm;
4658 memPtr->m_erp = float(m_joints[i]->m_erp);
4659 memPtr->m_split = float(m_joints[i]->m_split);
4660 memPtr->m_delete = m_joints[i]->m_delete;
4661
4662 for (int j = 0; j < 4; j++)
4663 {
4664 memPtr->m_relPosition[0].m_floats[j] = 0.f;
4665 memPtr->m_relPosition[1].m_floats[j] = 0.f;
4666 }
4667 memPtr->m_bodyA = 0;
4668 memPtr->m_bodyB = 0;
4669 if (m_joints[i]->m_bodies[0].m_soft)
4670 {
4671 memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
4672 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
4673 }
4674 if (m_joints[i]->m_bodies[0].m_collisionObject)
4675 {
4676 memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
4677 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
4678 }
4679 if (m_joints[i]->m_bodies[0].m_rigid)
4680 {
4681 memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
4682 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
4683 }
4684
4685 if (m_joints[i]->m_bodies[1].m_soft)
4686 {
4687 memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
4688 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
4689 }
4690 if (m_joints[i]->m_bodies[1].m_collisionObject)
4691 {
4692 memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
4693 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
4694 }
4695 if (m_joints[i]->m_bodies[1].m_rigid)
4696 {
4697 memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
4698 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
4699 }
4700 }
4701 serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]);
4702 }
4703
4704 return btSoftBodyDataName;
4705}
4706
4707void btSoftBody::updateDeactivation(btScalar timeStep)
4708{
4709 if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
4710 return;
4711
4712 if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
4713 {
4714 m_deactivationTime += timeStep;
4715 }
4716 else
4717 {
4718 m_deactivationTime = btScalar(0.);
4719 setActivationState(0);
4720 }
4721}
4722
4723void btSoftBody::setZeroVelocity()
4724{
4725 for (int i = 0; i < m_nodes.size(); ++i)
4726 {
4727 m_nodes[i].m_v.setZero();
4728 }
4729}
4730
4731bool btSoftBody::wantsSleeping()
4732{
4733 if (getActivationState() == DISABLE_DEACTIVATION)
4734 return false;
4735
4736 //disable deactivation
4737 if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
4738 return false;
4739
4740 if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
4741 return true;
4742
4743 if (m_deactivationTime > gDeactivationTime)
4744 {
4745 return true;
4746 }
4747 return false;
4748}
btAlignedFree
#define btAlignedFree(ptr)
Definition: btAlignedAllocator.h:47
btAlignedAlloc
#define btAlignedAlloc(size, alignment)
Definition: btAlignedAllocator.h:46
DISABLE_DEACTIVATION
#define DISABLE_DEACTIVATION
Definition: btCollisionObject.h:25
WANTS_DEACTIVATION
#define WANTS_DEACTIVATION
Definition: btCollisionObject.h:24
ISLAND_SLEEPING
#define ISLAND_SLEEPING
Definition: btCollisionObject.h:23
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
bounds
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:299
U
unsigned int U
Definition: btGjkEpa3.h:78
singularValueDecomposition
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'
Definition: btImplicitQRSVD.h:469
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
inverse
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btAngle
btScalar btAngle(const btQuaternion &q1, const btQuaternion &q2)
Return the angle between two quaternions.
Definition: btQuaternion.h:902
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
gDisableDeactivation
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
gDeactivationTime
btScalar gDeactivationTime
Definition: btRigidBody.cpp:25
btRigidBodyData
#define btRigidBodyData
Definition: btRigidBody.h:35
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:526
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
ATTRIBUTE_ALIGNED16
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btAcos
btScalar btAcos(btScalar x)
Definition: btScalar.h:501
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:643
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
btAssert
#define btAssert(x)
Definition: btScalar.h:153
BT_SBMATERIAL_CODE
#define BT_SBMATERIAL_CODE
Definition: btSerializer.h:119
BT_ARRAY_CODE
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
BT_SBNODE_CODE
#define BT_SBNODE_CODE
Definition: btSerializer.h:120
BT_JOINT_RIGID_BODY
@ BT_JOINT_RIGID_BODY
Definition: btSoftBodyData.h:166
BT_JOINT_SOFT_BODY_CLUSTER
@ BT_JOINT_SOFT_BODY_CLUSTER
Definition: btSoftBodyData.h:165
BT_JOINT_COLLISION_OBJECT
@ BT_JOINT_COLLISION_OBJECT
Definition: btSoftBodyData.h:167
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:95
OuterProduct
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
Definition: btSoftBodyInternals.h:924
Mul
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
Definition: btSoftBodyInternals.h:982
ImplicitSolve
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
Definition: btSoftBodyInternals.h:1199
VolumeOf
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
Definition: btSoftBodyInternals.h:1263
Lerp
static T Lerp(const T &a, const T &b, btScalar t)
Definition: btSoftBodyInternals.h:807
AngularImpulseMatrix
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
Definition: btSoftBodyInternals.h:1035
AreaOf
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
Definition: btSoftBodyInternals.h:1281
generateUnitOrthogonalVector
static btVector3 generateUnitOrthogonalVector(const btVector3 &u)
Definition: btSoftBodyInternals.h:47
NormalizeAny
static btVector3 NormalizeAny(const btVector3 &v)
Definition: btSoftBodyInternals.h:1253
PolarDecompose
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
Definition: btSoftBodyInternals.h:1411
MatchEdge
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
Definition: btSoftBodyInternals.h:1323
findJacobian
static void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
Definition: btSoftBodyInternals.h:34
SameSign
static bool SameSign(const T &x, const T &y)
Definition: btSoftBodyInternals.h:863
EvaluateMedium
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
Definition: btSoftBodyInternals.h:1234
ZeroInitialize
static void ZeroInitialize(T &value)
Definition: btSoftBodyInternals.h:789
ProjectOnPlane
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
Definition: btSoftBodyInternals.h:1048
ClusterMetric
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
Definition: btSoftBodyInternals.h:868
ImpulseMatrix
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
Definition: btSoftBodyInternals.h:1004
ApplyClampedForce
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
Definition: btSoftBodyInternals.h:1307
Clamp
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
Definition: btSoftBodyInternals.h:829
Diagonal
static btMatrix3x3 Diagonal(btScalar x)
Definition: btSoftBodyInternals.h:898
calculateNormalCone
static void calculateNormalCone(btDbvntNode *root)
Definition: btSoftBody.cpp:2611
IDX
#define IDX(_x_, _y_)
IDX2PTR
#define IDX2PTR(_p_, _b_)
PTR2IDX
#define PTR2IDX(_p_, _b_)
getBarycentric
static void getBarycentric(const btVector3 &p, btVector3 &a, btVector3 &b, btVector3 &c, btVector3 &bary)
Definition: btSoftBody.cpp:2810
Dot4
static btScalar Dot4(const btVector4 &a, const btVector4 &b)
Definition: btSoftBody.cpp:3519
NEXTRAND
#define NEXTRAND
buildTreeBottomUp
static btDbvtNode * buildTreeBottomUp(btAlignedObjectArray< btDbvtNode * > &leafNodes, btAlignedObjectArray< btAlignedObjectArray< int > > &adj)
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.cpp:29
copyToDbvnt
static btDbvntNode * copyToDbvnt(const btDbvtNode *n)
Definition: btSoftBody.cpp:2596
seed
static unsigned long seed
Definition: btSoftBody.h:39
btSoftBodyDataName
#define btSoftBodyDataName
Definition: btSoftBody.h:37
btSoftBodyData
#define btSoftBodyData
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.h:36
SAFE_EPSILON
#define SAFE_EPSILON
Definition: btSoftBody.h:1224
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
btVector3Data
#define btVector3Data
Definition: btVector3.h:27
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:46
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::findLinearSearch
int findLinearSearch(const T &key) const
Definition: btAlignedObjectArray.h:438
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btAlignedObjectArray::clear
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
Definition: btAlignedObjectArray.h:176
btAlignedObjectArray::remove
void remove(const T &key)
Definition: btAlignedObjectArray.h:480
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btAlignedObjectArray::capacity
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
Definition: btAlignedObjectArray.h:275
btBroadphaseInterface::setAabb
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)=0
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:52
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:51
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:207
btCollisionObject::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btCollisionObject.cpp:81
btCollisionObject::getWorldTransform
btTransform & getWorldTransform()
Definition: btCollisionObject.h:379
btCollisionObject::getBroadphaseHandle
btBroadphaseProxy * getBroadphaseHandle()
Definition: btCollisionObject.h:395
btCollisionObject::m_worldTransform
btTransform m_worldTransform
Definition: btCollisionObject.h:53
btCollisionObject::m_collisionShape
btCollisionShape * m_collisionShape
Definition: btCollisionObject.h:68
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:374
btCollisionObject::hasContactResponse
bool hasContactResponse() const
Definition: btCollisionObject.h:212
btCollisionObject::activate
void activate(bool forceActivation=false) const
Definition: btCollisionObject.cpp:72
btCollisionObject::setActivationState
void setActivationState(int newState) const
Definition: btCollisionObject.cpp:61
btCollisionObject::m_internalType
int m_internalType
m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody,...
Definition: btCollisionObject.h:95
btCollisionObject::getFriction
btScalar getFriction() const
Definition: btCollisionObject.h:325
btCollisionObject::getInterpolationWorldTransform
const btTransform & getInterpolationWorldTransform() const
Definition: btCollisionObject.h:410
btCollisionObject::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObject.h:228
btCollisionObject::getActivationState
int getActivationState() const
Definition: btCollisionObject.h:289
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
Definition: btCollisionShape.h:28
btCollisionShape::setMargin
virtual void setMargin(btScalar margin)=0
btCollisionShape::getMargin
virtual btScalar getMargin() const =0
btCollisionShape::getAabb
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.
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
btConvexShape::getMargin
virtual btScalar getMargin() const =0
btHashMap
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:220
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
btHashMap::find
const Value * find(const Key &key) const
Definition: btHashMap.h:424
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3::adjoint
btMatrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: btMatrix3x3.h:1085
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1093
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:323
btMatrix3x3::determinant
btScalar determinant() const
Return the determinant of the matrix.
Definition: btMatrix3x3.h:1022
btMatrix3x3::extractRotation
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...
Definition: btMatrix3x3.h:688
btMatrix3x3::setZero
void setZero()
Set the matrix to the identity.
Definition: btMatrix3x3.h:337
btMatrix3x3::serializeFloat
void serializeFloat(struct btMatrix3x3FloatData &dataOut) const
Definition: btMatrix3x3.h:1407
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:61
btMultiBody::fillContactJacobianMultiDof
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.h:504
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
Definition: btMultiBody.cpp:1432
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:465
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:302
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btQuaternion::setRotation
void setRotation(const btVector3 &axis, const btScalar &_angle)
Set the rotation using axis angle notation.
Definition: btQuaternion.h:108
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btRigidBody::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btRigidBody.h:242
btRigidBody::applyImpulse
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:335
btRigidBody::upcast
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btSerializer::findPointer
virtual void * findPointer(void *oldPtr)=0
btSoftBody::DeformableRigidContact::jacobianData_t1
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:372
btSoftBody::DeformableRigidContact::jacobianData_normal
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:371
btSoftBody::DeformableRigidContact::jacobianData_t2
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:373
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
btSoftBody::PSolve_Links
static void PSolve_Links(btSoftBody *psb, btScalar kst, btScalar ti)
Definition: btSoftBody.cpp:3995
btSoftBody::PSolve_SContacts
static void PSolve_SContacts(btSoftBody *psb, btScalar, btScalar ti)
Definition: btSoftBody.cpp:3960
btSoftBody::checkLink
bool checkLink(int node0, int node1) const
Definition: btSoftBody.cpp:256
btSoftBody::m_bUpdateRtCst
bool m_bUpdateRtCst
Definition: btSoftBody.h:834
btSoftBody::m_sleepingThreshold
btScalar m_sleepingThreshold
Definition: btSoftBody.h:841
btSoftBody::transformTo
virtual void transformTo(const btTransform &trs)
Definition: btSoftBody.cpp:1093
btSoftBody::getLinearVelocity
btVector3 getLinearVelocity()
Definition: btSoftBody.cpp:1034
btSoftBody::checkFace
bool checkFace(int node0, int node1, int node2) const
Definition: btSoftBody.cpp:278
btSoftBody::advanceDeformation
void advanceDeformation()
Definition: btSoftBody.cpp:3562
btSoftBody::setGravityFactor
void setGravityFactor(btScalar gravFactor)
Definition: btSoftBody.cpp:3462
btSoftBody::updateClusters
void updateClusters()
Definition: btSoftBody.cpp:3229
btSoftBody::m_cdbvt
btDbvt m_cdbvt
Definition: btSoftBody.h:838
btSoftBody::setPose
void setPose(bool bvolume, bool bframe)
Definition: btSoftBody.cpp:1185
btSoftBody::cutLink
bool cutLink(int node0, int node1, btScalar position)
Definition: btSoftBody.cpp:1984
btSoftBody::appendFace
void appendFace(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:432
btSoftBody::setMass
void setMass(int node, btScalar mass)
Definition: btSoftBody.cpp:923
btSoftBody::interpolateRenderMesh
void interpolateRenderMesh()
Definition: btSoftBody.cpp:3815
btSoftBody::m_joints
tJointArray m_joints
Definition: btSoftBody.h:830
btSoftBody::m_dampingCoefficient
btScalar m_dampingCoefficient
Definition: btSoftBody.h:840
btSoftBody::m_tetraScratchesTn
btAlignedObjectArray< TetraScratch > m_tetraScratchesTn
Definition: btSoftBody.h:821
btSoftBody::integrateMotion
void integrateMotion()
Definition: btSoftBody.cpp:2348
btSoftBody::rebuildNodeTree
void rebuildNodeTree()
Definition: btSoftBody.cpp:2697
btSoftBody::rayFaceTest
bool rayFaceTest(const btVector3 &rayFrom, const btVector3 &rayTo, sRayCast &results)
Definition: btSoftBody.cpp:2054
btSoftBody::appendLinearJoint
void appendLinearJoint(const LJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:640
btSoftBody::scale
virtual void scale(const btVector3 &scl)
Definition: btSoftBody.cpp:1143
btSoftBody::m_clusterConnectivity
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:855
btSoftBody::updateFaceTree
void updateFaceTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1300
btSoftBody::defaultCollisionHandler
void defaultCollisionHandler(const btCollisionObjectWrapper *pcoWrap)
Definition: btSoftBody.cpp:4076
btSoftBody::getVolume
btScalar getVolume() const
Definition: btSoftBody.cpp:1244
btSoftBody::rayTest
bool rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, sRayCast &results)
Ray casting using rayFrom and rayTo in worldspace, (not direction!)
Definition: btSoftBody.cpp:2039
btSoftBody::m_sst
SolverState m_sst
Definition: btSoftBody.h:809
btSoftBody::addVelocity
void addVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:893
btSoftBody::predictMotion
void predictMotion(btScalar dt)
Definition: btSoftBody.cpp:2099
btSoftBody::setSelfCollision
void setSelfCollision(bool useSelfCollision)
Definition: btSoftBody.cpp:4065
btSoftBody::setLinearVelocity
void setLinearVelocity(const btVector3 &linVel)
Definition: btSoftBody.cpp:1047
btSoftBody::m_timeacc
btScalar m_timeacc
Definition: btSoftBody.h:832
btSoftBody::m_pose
Pose m_pose
Definition: btSoftBody.h:810
btSoftBody::appendTetra
void appendTetra(int model, Material *mat)
Definition: btSoftBody.cpp:472
btSoftBody::setRestLengthScale
void setRestLengthScale(btScalar restLength)
Definition: btSoftBody.cpp:1170
btSoftBody::m_fdbvnt
btDbvntNode * m_fdbvnt
Definition: btSoftBody.h:837
btSoftBody::rotate
virtual void rotate(const btQuaternion &rot)
Definition: btSoftBody.cpp:1134
btSoftBody::applyClusters
void applyClusters(bool drift)
Definition: btSoftBody.cpp:3377
btSoftBody::setZeroVelocity
void setZeroVelocity()
Definition: btSoftBody.cpp:4723
btSoftBody::PSolve_Anchors
static void PSolve_Anchors(btSoftBody *psb, btScalar kst, btScalar ti)
Definition: btSoftBody.cpp:3864
btSoftBody::m_worldInfo
btSoftBodyWorldInfo * m_worldInfo
Definition: btSoftBody.h:812
btSoftBody::updateArea
void updateArea(bool averageArea=true)
Definition: btSoftBody.cpp:3077
btSoftBody::addForce
void addForce(const btVector3 &force)
Definition: btSoftBody.cpp:693
btSoftBody::wantsSleeping
bool wantsSleeping()
Definition: btSoftBody.cpp:4731
btSoftBody::prepareClusters
void prepareClusters(int iterations)
Definition: btSoftBody.cpp:3359
btSoftBody::setCollisionQuadrature
void setCollisionQuadrature(int N)
Definition: btSoftBody.cpp:3852
btSoftBody::clusterVImpulse
static void clusterVImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
Definition: btSoftBody.cpp:1292
btSoftBody::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btSoftBody.cpp:4327
btSoftBody::VSolve_Links
static void VSolve_Links(btSoftBody *psb, btScalar kst)
Definition: btSoftBody.cpp:4018
btSoftBody::m_tetras
tTetraArray m_tetras
Definition: btSoftBody.h:819
btSoftBody::useSelfCollision
bool useSelfCollision()
Definition: btSoftBody.cpp:4070
btSoftBody::evaluateCom
btVector3 evaluateCom() const
Definition: btSoftBody.cpp:2735
btSoftBody::setTotalDensity
void setTotalDensity(btScalar density)
Definition: btSoftBody.cpp:983
btSoftBody::m_collisionDisabledObjects
btAlignedObjectArray< const class btCollisionObject * > m_collisionDisabledObjects
Definition: btSoftBody.h:77
btSoftBody::clusterDAImpulse
static void clusterDAImpulse(Cluster *cluster, const btVector3 &impulse)
Definition: btSoftBody.cpp:1330
btSoftBody::appendNode
void appendNode(const btVector3 &x, btScalar m)
Definition: btSoftBody.cpp:372
btSoftBody::staticSolve
void staticSolve(int iterations)
Definition: btSoftBody.cpp:2301
btSoftBody::setVolumeMass
void setVolumeMass(btScalar mass)
Definition: btSoftBody.cpp:989
btSoftBody::m_restLengthScale
btScalar m_restLengthScale
Definition: btSoftBody.h:859
btSoftBody::checkDeformableContact
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Definition: btSoftBody.cpp:2777
btSoftBody::m_reducedModel
bool m_reducedModel
Definition: btSoftBody.h:861
btSoftBody::cleanupClusters
void cleanupClusters()
Definition: btSoftBody.cpp:3345
btSoftBody::m_cfg
Config m_cfg
Definition: btSoftBody.h:808
btSoftBody::updateDeactivation
void updateDeactivation(btScalar timeStep)
Definition: btSoftBody.cpp:4707
btSoftBody::m_tetraScratches
btAlignedObjectArray< TetraScratch > m_tetraScratches
Definition: btSoftBody.h:820
btSoftBody::getWindVelocity
const btVector3 & getWindVelocity()
Return the wind velocity for interaction with the air.
Definition: btSoftBody.cpp:4315
btSoftBody::addAeroForceToFace
void addAeroForceToFace(const btVector3 &windVelocity, int faceIndex)
Definition: btSoftBody.cpp:797
btSoftBody::m_renderNodesInterpolationWeights
btAlignedObjectArray< btVector4 > m_renderNodesInterpolationWeights
Definition: btSoftBody.h:849
btSoftBody::appendAngularJoint
void appendAngularJoint(const AJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:666
btSoftBody::m_faces
tFaceArray m_faces
Definition: btSoftBody.h:817
btSoftBody::setAngularVelocity
void setAngularVelocity(const btVector3 &angVel)
Definition: btSoftBody.cpp:1056
btSoftBody::setVolumeDensity
void setVolumeDensity(btScalar density)
Definition: btSoftBody.cpp:1019
btSoftBody::clusterDCImpulse
static void clusterDCImpulse(Cluster *cluster, const btVector3 &impulse)
Definition: btSoftBody.cpp:1345
btSoftBody::transform
virtual void transform(const btTransform &trs)
Definition: btSoftBody.cpp:1103
btSoftBody::m_softSoftCollision
bool m_softSoftCollision
Definition: btSoftBody.h:853
btSoftBody::clusterVAImpulse
static void clusterVAImpulse(Cluster *cluster, const btVector3 &impulse)
Definition: btSoftBody.cpp:1321
btSoftBody::getMass
btScalar getMass(int node) const
Definition: btSoftBody.cpp:930
btSoftBody::m_materials
tMaterialArray m_materials
Definition: btSoftBody.h:831
btSoftBody::setMaxStress
void setMaxStress(btScalar maxStress)
Definition: btSoftBody.cpp:3809
btSoftBody::dampClusters
void dampClusters()
Definition: btSoftBody.cpp:3428
btSoftBody::btSoftBody
btSoftBody(btSoftBodyWorldInfo *worldInfo, int node_count, const btVector3 *x, const btScalar *m)
Definition: btSoftBody.cpp:130
btSoftBody::updateDeformation
void updateDeformation()
Definition: btSoftBody.cpp:3524
btSoftBody::m_z
btAlignedObjectArray< btScalar > m_z
Definition: btSoftBody.h:851
btSoftBody::addAeroForceToNode
void addAeroForceToNode(const btVector3 &windVelocity, int nodeIndex)
Definition: btSoftBody.cpp:708
btSoftBody::clusterCom
static btVector3 clusterCom(const Cluster *cluster)
Definition: btSoftBody.cpp:1269
btSoftBody::m_rcontacts
tRContactArray m_rcontacts
Definition: btSoftBody.h:824
btSoftBody::appendAnchor
void appendAnchor(int node, btRigidBody *body, bool disableCollisionBetweenLinkedBodies=false, btScalar influence=1)
Definition: btSoftBody.cpp:504
btSoftBody::m_bounds
btVector3 m_bounds[2]
Definition: btSoftBody.h:833
btSoftBody::m_maxSpeedSquared
btScalar m_maxSpeedSquared
Definition: btSoftBody.h:842
btSoftBody::releaseCluster
void releaseCluster(int index)
Definition: btSoftBody.cpp:1505
btSoftBody::m_repulsionStiffness
btScalar m_repulsionStiffness
Definition: btSoftBody.h:844
btSoftBody::setVelocity
void setVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:899
btSoftBody::m_clusters
tClusterArray m_clusters
Definition: btSoftBody.h:839
btSoftBody::solveConstraints
void solveConstraints()
Definition: btSoftBody.cpp:2208
btSoftBody::generateClusters
int generateClusters(int k, int maxiterations=8192)
generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle otherwise an ...
Definition: btSoftBody.cpp:1521
btSoftBody::geometricCollisionHandler
void geometricCollisionHandler(btSoftBody *psb)
Definition: btSoftBody.cpp:4258
btSoftBody::releaseClusters
void releaseClusters()
Definition: btSoftBody.cpp:1515
btSoftBody::refine
void refine(ImplicitFn *ifn, btScalar accurary, bool cut)
Definition: btSoftBody.cpp:1714
btSoftBody::setSolver
void setSolver(eSolverPresets::_ preset)
Definition: btSoftBody.cpp:2074
btSoftBody::BaryEval
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
Definition: btSoftBody.h:1309
btSoftBody::appendMaterial
Material * appendMaterial()
Definition: btSoftBody.cpp:302
btSoftBody::removeAnchor
void removeAnchor(int node)
Definition: btSoftBody.cpp:566
btSoftBody::m_deformableAnchors
btAlignedObjectArray< DeformableNodeRigidAnchor > m_deformableAnchors
Definition: btSoftBody.h:823
btSoftBody::setCacheBarycenter
void setCacheBarycenter(bool cacheBarycenter)
Definition: btSoftBody.cpp:3467
btSoftBody::m_gravityFactor
btScalar m_gravityFactor
Definition: btSoftBody.h:845
btSoftBody::solveClusters
static void solveClusters(const btAlignedObjectArray< btSoftBody * > &bodies)
Definition: btSoftBody.cpp:2319
btSoftBody::appendNote
void appendNote(const char *text, const btVector3 &o, const btVector4 &c=btVector4(1, 0, 0, 0), Node *n0=0, Node *n1=0, Node *n2=0, Node *n3=0)
Definition: btSoftBody.cpp:314
btSoftBody::checkDeformableFaceContact
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
Definition: btSoftBody.cpp:2825
btSoftBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btSoftBody.cpp:4320
btSoftBody::PSolve_RContacts
static void PSolve_RContacts(btSoftBody *psb, btScalar kst, btScalar ti)
Definition: btSoftBody.cpp:3885
btSoftBody::clusterImpulse
static void clusterImpulse(Cluster *cluster, const btVector3 &rpos, const Impulse &impulse)
Definition: btSoftBody.cpp:1314
btSoftBody::m_renderNodesParents
btAlignedObjectArray< btAlignedObjectArray< const btSoftBody::Node * > > m_renderNodesParents
Definition: btSoftBody.h:850
btSoftBody::m_renderNodes
tRenderNodeArray m_renderNodes
Definition: btSoftBody.h:815
btSoftBody::pointersToIndices
void pointersToIndices()
Definition: btSoftBody.cpp:2416
btSoftBody::m_notes
tNoteArray m_notes
Definition: btSoftBody.h:813
btSoftBody::updateNormals
void updateNormals()
Definition: btSoftBody.cpp:2937
btSoftBody::clusterDImpulse
static void clusterDImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
Definition: btSoftBody.cpp:1304
btSoftBody::m_fdbvt
btDbvt m_fdbvt
Definition: btSoftBody.h:836
btSoftBody::m_links
tLinkArray m_links
Definition: btSoftBody.h:816
btSoftBody::applyForces
void applyForces()
Definition: btSoftBody.cpp:3741
btSoftBody::clusterVelocity
static btVector3 clusterVelocity(const Cluster *cluster, const btVector3 &rpos)
Definition: btSoftBody.cpp:1286
btSoftBody::getRigidTransform
btTransform getRigidTransform()
Definition: btSoftBody.cpp:1067
btSoftBody::m_scontacts
tSContactArray m_scontacts
Definition: btSoftBody.h:829
btSoftBody::m_cacheBarycenter
bool m_cacheBarycenter
Definition: btSoftBody.h:846
btSoftBody::m_useSelfCollision
bool m_useSelfCollision
Definition: btSoftBody.h:852
btSoftBody::m_tag
void * m_tag
Definition: btSoftBody.h:811
btSoftBody::updatePose
void updatePose()
Definition: btSoftBody.cpp:3040
btSoftBody::psolver_t
void(* psolver_t)(btSoftBody *, btScalar, btScalar)
Definition: btSoftBody.h:785
btSoftBody::initializeClusters
void initializeClusters()
Definition: btSoftBody.cpp:3162
btSoftBody::m_anchors
tAnchorArray m_anchors
Definition: btSoftBody.h:822
btSoftBody::getRestLengthScale
btScalar getRestLengthScale()
Definition: btSoftBody.cpp:1164
btSoftBody::updateState
void updateState(const btAlignedObjectArray< btVector3 > &qs, const btAlignedObjectArray< btVector3 > &vs)
Definition: btSoftBody.cpp:1489
btSoftBody::randomizeConstraints
void randomizeConstraints()
Definition: btSoftBody.cpp:1472
btSoftBody::m_windVelocity
btVector3 m_windVelocity
Definition: btSoftBody.h:857
btSoftBody::getTotalMass
btScalar getTotalMass() const
Definition: btSoftBody.cpp:936
btSoftBody::m_nodes
tNodeArray m_nodes
Definition: btSoftBody.h:814
btSoftBody::appendLink
void appendLink(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:392
btSoftBody::setSpringStiffness
void setSpringStiffness(btScalar k)
Definition: btSoftBody.cpp:3453
btSoftBody::initializeDmInverse
void initializeDmInverse()
Definition: btSoftBody.cpp:3472
btSoftBody::updateConstants
void updateConstants()
Definition: btSoftBody.cpp:3154
btSoftBody::setTotalMass
void setTotalMass(btScalar mass, bool fromfaces=false)
Definition: btSoftBody.cpp:947
btSoftBody::~btSoftBody
virtual ~btSoftBody()
Definition: btSoftBody.cpp:240
btSoftBody::appendDeformableAnchor
void appendDeformableAnchor(int node, btRigidBody *body)
Definition: btSoftBody.cpp:531
btSoftBody::updateLinkConstants
void updateLinkConstants()
Definition: btSoftBody.cpp:3141
btSoftBody::vsolver_t
void(* vsolver_t)(btSoftBody *, btScalar)
Definition: btSoftBody.h:786
btSoftBody::initDefaults
void initDefaults()
Definition: btSoftBody.cpp:170
btSoftBody::m_quads
btAlignedObjectArray< btVector3 > m_quads
Definition: btSoftBody.h:843
btSoftBody::getSolver
static psolver_t getSolver(ePSolver::_ solver)
Definition: btSoftBody.cpp:4032
btSoftBody::checkContact
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
Definition: btSoftBody.cpp:2748
btSoftBody::indicesToPointers
void indicesToPointers(const int *map=0)
Definition: btSoftBody.cpp:2459
btSoftBody::solveCommonConstraints
static void solveCommonConstraints(btSoftBody **bodies, int count, int iterations)
Definition: btSoftBody.cpp:2313
btSoftBody::updateBounds
void updateBounds()
Definition: btSoftBody.cpp:2966
btSoftBody::setWindVelocity
void setWindVelocity(const btVector3 &velocity)
Set a wind velocity for interaction with the air.
Definition: btSoftBody.cpp:4310
btSoftBody::generateBendingConstraints
int generateBendingConstraints(int distance, Material *mat=0)
Definition: btSoftBody.cpp:1357
btSoftBody::m_X
btAlignedObjectArray< btVector3 > m_X
Definition: btSoftBody.h:847
btSoftBody::translate
virtual void translate(const btVector3 &trs)
Definition: btSoftBody.cpp:1125
btSoftBody::getCenterOfMass
btVector3 getCenterOfMass() const
Definition: btSoftBody.h:1030
btSoftBody::initializeFaceTree
void initializeFaceTree()
Definition: btSoftBody.cpp:2642
btSoftBody::resetLinkRestLengths
void resetLinkRestLengths()
Definition: btSoftBody.cpp:1233
btSoftBody::clusterCount
int clusterCount() const
Definition: btSoftBody.cpp:1263
btSoftBody::m_ndbvt
btDbvt m_ndbvt
Definition: btSoftBody.h:835
btSoftBody::clusterAImpulse
static void clusterAImpulse(Cluster *cluster, const Impulse &impulse)
Definition: btSoftBody.cpp:1338
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:183
btTransform::invXform
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:216
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:109
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:161
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:167
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:114
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:147
btTransform::setBasis
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:155
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVector3::setMax
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btVector3::w
const btScalar & w() const
Return the w value.
Definition: btVector3.h:581
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3::safeNormalize
btVector3 & safeNormalize()
Definition: btVector3.h:286
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::serializeFloat
void serializeFloat(struct btVector3FloatData &dataOut) const
Definition: btVector3.h:1291
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btVector3::setMin
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:626
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
eps
const btScalar eps
Definition: poly34.cpp:11
SoftBodyClusterData::m_av
btVector3FloatData m_av
Definition: btSoftBodyData.h:138
SoftBodyClusterData::m_invwi
btMatrix3x3FloatData m_invwi
Definition: btSoftBodyData.h:133
SoftBodyClusterData::m_vimpulses
btVector3FloatData m_vimpulses[2]
Definition: btSoftBodyData.h:135
SoftBodyClusterData::m_framerefs
btVector3FloatData * m_framerefs
Definition: btSoftBodyData.h:140
SoftBodyClusterData::m_dimpulses
btVector3FloatData m_dimpulses[2]
Definition: btSoftBodyData.h:136
SoftBodyClusterData::m_locii
btMatrix3x3FloatData m_locii
Definition: btSoftBodyData.h:132
SoftBodyClusterData::m_lv
btVector3FloatData m_lv
Definition: btSoftBodyData.h:137
SoftBodyClusterData::m_framexform
btTransformFloatData m_framexform
Definition: btSoftBodyData.h:131
SoftBodyClusterData::m_selfCollisionImpulseFactor
float m_selfCollisionImpulseFactor
Definition: btSoftBodyData.h:157
SoftBodyClusterData::m_com
btVector3FloatData m_com
Definition: btSoftBodyData.h:134
SoftBodyFaceData::m_normal
btVector3FloatData m_normal
Definition: btSoftBodyData.h:54
SoftBodyFaceData::m_material
SoftBodyMaterialData * m_material
Definition: btSoftBodyData.h:55
SoftBodyLinkData::m_material
SoftBodyMaterialData * m_material
Definition: btSoftBodyData.h:46
SoftBodyNodeData::m_accumulatedForce
btVector3FloatData m_accumulatedForce
Definition: btSoftBodyData.h:36
SoftBodyNodeData::m_normal
btVector3FloatData m_normal
Definition: btSoftBodyData.h:37
SoftBodyNodeData::m_previousPosition
btVector3FloatData m_previousPosition
Definition: btSoftBodyData.h:34
SoftBodyNodeData::m_velocity
btVector3FloatData m_velocity
Definition: btSoftBodyData.h:35
SoftBodyNodeData::m_position
btVector3FloatData m_position
Definition: btSoftBodyData.h:33
SoftBodyNodeData::m_material
SoftBodyMaterialData * m_material
Definition: btSoftBodyData.h:32
SoftBodyPoseData::m_rot
btMatrix3x3FloatData m_rot
Definition: btSoftBodyData.h:113
SoftBodyPoseData::m_com
btVector3FloatData m_com
Definition: btSoftBodyData.h:116
SoftBodyPoseData::m_positions
btVector3FloatData * m_positions
Definition: btSoftBodyData.h:118
SoftBodyPoseData::m_aqq
btMatrix3x3FloatData m_aqq
Definition: btSoftBodyData.h:115
SoftBodyPoseData::m_scale
btMatrix3x3FloatData m_scale
Definition: btSoftBodyData.h:114
SoftBodyTetraData::m_c0
btVector3FloatData m_c0[4]
Definition: btSoftBodyData.h:62
SoftBodyTetraData::m_material
SoftBodyMaterialData * m_material
Definition: btSoftBodyData.h:63
SoftRigidAnchorData::m_c0
btMatrix3x3FloatData m_c0
Definition: btSoftBodyData.h:73
SoftRigidAnchorData::m_localFrame
btVector3FloatData m_localFrame
Definition: btSoftBodyData.h:75
SoftRigidAnchorData::m_c1
btVector3FloatData m_c1
Definition: btSoftBodyData.h:74
SoftRigidAnchorData::m_rigidBody
btRigidBodyData * m_rigidBody
Definition: btSoftBodyData.h:76
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:46
btCollisionObjectWrapper::m_preTransform
const btTransform * m_preTransform
Definition: btCollisionObjectWrapper.h:30
btCollisionObjectWrapper::getCollisionObject
const btCollisionObject * getCollisionObject() const
Definition: btCollisionObjectWrapper.h:45
btCollisionObjectWrapper::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btCollisionObjectWrapper.h:44
btDbvntNode::data
void * data
Definition: btDbvt.h:202
btDbvntNode::normal
btVector3 normal
Definition: btDbvt.h:197
btDbvntNode::childs
btDbvntNode * childs[2]
Definition: btDbvt.h:201
btDbvntNode::angle
btScalar angle
Definition: btDbvt.h:198
btDbvntNode::isleaf
DBVT_INLINE bool isleaf() const
Definition: btDbvt.h:199
btDbvtAabbMm::FromMM
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
btDbvtAabbMm::FromCR
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
Definition: btDbvt.h:473
btDbvtNode::isinternal
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:185
btDbvtNode::childs
btDbvtNode * childs[2]
Definition: btDbvt.h:187
btDbvtNode::parent
btDbvtNode * parent
Definition: btDbvt.h:183
btDbvtNode::data
void * data
Definition: btDbvt.h:188
btDbvtNode::volume
btDbvtVolume volume
Definition: btDbvt.h:182
btDbvt::insert
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:535
btDbvt::optimizeIncremental
void optimizeIncremental(int passes)
Definition: btDbvt.cpp:514
btDbvt::update
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:544
btDbvt::collideTV
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:1148
btDbvt::empty
bool empty() const
Definition: btDbvt.h:314
btDbvt::rayTest
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
btDbvt::clear
void clear()
Definition: btDbvt.cpp:477
btDbvt::m_root
btDbvtNode * m_root
Definition: btDbvt.h:302
btDbvt::remove
void remove(btDbvtNode *leaf)
Definition: btDbvt.cpp:611
btDbvt::collideTT
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:822
btDbvt::selfCollideT
DBVT_PREFIX void selfCollideT(const btDbvntNode *root, DBVT_IPOLICY)
Definition: btDbvt.h:887
btGjkEpaSolver2::sResults::witnesses
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
btGjkEpaSolver2::SignedDistance
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:46
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:45
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:48
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:50
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:49
btSoftBodyJointData::m_refs
btVector3FloatData m_refs[2]
Definition: btSoftBodyData.h:174
btSoftBodyJointData::m_relPosition
btVector3FloatData m_relPosition[2]
Definition: btSoftBodyData.h:179
btSoftBodyWorldInfo::air_density
btScalar air_density
Definition: btSoftBody.h:49
btSoftBodyWorldInfo::m_dispatcher
btDispatcher * m_dispatcher
Definition: btSoftBody.h:55
btSoftBodyWorldInfo::m_sparsesdf
btSparseSdf< 3 > m_sparsesdf
Definition: btSoftBody.h:57
btSoftBodyWorldInfo::m_gravity
btVector3 m_gravity
Definition: btSoftBody.h:56
btSoftBodyWorldInfo::m_maxDisplacement
btScalar m_maxDisplacement
Definition: btSoftBody.h:52
btSoftBodyWorldInfo::m_broadphase
btBroadphaseInterface * m_broadphase
Definition: btSoftBody.h:54
btSoftBody::AJoint::Prepare
void Prepare(btScalar dt, int iterations)
Definition: btSoftBody.cpp:3621
btSoftBody::AJoint::Solve
void Solve(btScalar dt, btScalar sor)
Definition: btSoftBody.cpp:3641
btSoftBody::AJoint::m_icontrol
IControl * m_icontrol
Definition: btSoftBody.h:688
btSoftBody::AJoint::Terminate
void Terminate(btScalar dt)
Definition: btSoftBody.cpp:3656
btSoftBody::Anchor::m_influence
btScalar m_influence
Definition: btSoftBody.h:430
btSoftBody::Anchor::m_local
btVector3 m_local
Definition: btSoftBody.h:428
btSoftBody::Anchor::m_body
btRigidBody * m_body
Definition: btSoftBody.h:429
btSoftBody::Anchor::m_c0
btMatrix3x3 m_c0
Definition: btSoftBody.h:431
btSoftBody::Body::xform
const btTransform & xform() const
Definition: btSoftBody.h:546
btSoftBody::CJoint::Terminate
void Terminate(btScalar dt)
Definition: btSoftBody.cpp:3731
btSoftBody::CJoint::Prepare
void Prepare(btScalar dt, int iterations)
Definition: btSoftBody.cpp:3666
btSoftBody::CJoint::Solve
void Solve(btScalar dt, btScalar sor)
Definition: btSoftBody.cpp:3688
btSoftBody::Cluster::m_dimpulses
btVector3 m_dimpulses[2]
Definition: btSoftBody.h:470
btSoftBody::Cluster::m_framerefs
tVector3Array m_framerefs
Definition: btSoftBody.h:462
btSoftBody::Cluster::m_invwi
btMatrix3x3 m_invwi
Definition: btSoftBody.h:467
btSoftBody::Cluster::m_locii
btMatrix3x3 m_locii
Definition: btSoftBody.h:466
btSoftBody::Cluster::m_nodes
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:461
btSoftBody::Cluster::m_leaf
btDbvtNode * m_leaf
Definition: btSoftBody.h:475
btSoftBody::Cluster::m_vimpulses
btVector3 m_vimpulses[2]
Definition: btSoftBody.h:469
btSoftBody::Cluster::m_masses
tScalarArray m_masses
Definition: btSoftBody.h:460
btSoftBody::Cluster::m_framexform
btTransform m_framexform
Definition: btSoftBody.h:463
btSoftBody::Config::maxvolume
btScalar maxvolume
Definition: btSoftBody.h:729
btSoftBody::Config::m_psequence
tPSolverArray m_psequence
Definition: btSoftBody.h:737
btSoftBody::Config::m_dsequence
tPSolverArray m_dsequence
Definition: btSoftBody.h:738
btSoftBody::Config::kSK_SPLT_CL
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:727
btSoftBody::Config::kSS_SPLT_CL
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:728
btSoftBody::Config::m_maxStress
btScalar m_maxStress
Definition: btSoftBody.h:740
btSoftBody::Config::aeromodel
eAeroModel::_ aeromodel
Definition: btSoftBody.h:710
btSoftBody::Config::kSR_SPLT_CL
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:726
btSoftBody::Config::m_vsequence
tVSolverArray m_vsequence
Definition: btSoftBody.h:736
btSoftBody::Config::timescale
btScalar timescale
Definition: btSoftBody.h:730
btSoftBody::Face::m_pcontact
btVector4 m_pcontact
Definition: btSoftBody.h:311
btSoftBody::Face::m_normal
btVector3 m_normal
Definition: btSoftBody.h:308
btSoftBody::Face::m_n
Node * m_n[3]
Definition: btSoftBody.h:307
btSoftBody::Face::m_leaf
btDbvtNode * m_leaf
Definition: btSoftBody.h:310
btSoftBody::Feature::m_material
Material * m_material
Definition: btSoftBody.h:258
btSoftBody::ImplicitFn::Eval
virtual btScalar Eval(const btVector3 &x)=0
btSoftBody::Impulse::m_velocity
btVector3 m_velocity
Definition: btSoftBody.h:492
btSoftBody::Joint::m_split
btScalar m_split
Definition: btSoftBody.h:642
btSoftBody::Joint::m_refs
btVector3 m_refs[2]
Definition: btSoftBody.h:639
btSoftBody::Joint::Prepare
virtual void Prepare(btScalar dt, int iterations)
Definition: btSoftBody.cpp:3571
btSoftBody::LJoint::Solve
void Solve(btScalar dt, btScalar sor)
Definition: btSoftBody.cpp:3598
btSoftBody::LJoint::Prepare
void Prepare(btScalar dt, int iterations)
Definition: btSoftBody.cpp:3578
btSoftBody::LJoint::Terminate
void Terminate(btScalar dt)
Definition: btSoftBody.cpp:3611
btSoftBody::Node::m_area
btScalar m_area
Definition: btSoftBody.h:276
btSoftBody::Node::m_x
btVector3 m_x
Definition: btSoftBody.h:269
btSoftBody::Node::m_vn
btVector3 m_vn
Definition: btSoftBody.h:272
btSoftBody::Node::m_v
btVector3 m_v
Definition: btSoftBody.h:271
btSoftBody::Node::m_q
btVector3 m_q
Definition: btSoftBody.h:270
btSoftBody::Node::m_leaf
btDbvtNode * m_leaf
Definition: btSoftBody.h:277
btSoftBody::Node::m_n
btVector3 m_n
Definition: btSoftBody.h:274
btSoftBody::Node::m_f
btVector3 m_f
Definition: btSoftBody.h:273
btSoftBody::Note::m_coords
btScalar m_coords[4]
Definition: btSoftBody.h:442
btSoftBody::Note::m_offset
btVector3 m_offset
Definition: btSoftBody.h:439
btSoftBody::Note::m_nodes
Node * m_nodes[4]
Definition: btSoftBody.h:441
btSoftBody::Note::m_text
const char * m_text
Definition: btSoftBody.h:438
btSoftBody::Pose::m_scl
btMatrix3x3 m_scl
Definition: btSoftBody.h:454
btSoftBody::Pose::m_volume
btScalar m_volume
Definition: btSoftBody.h:449
btSoftBody::Pose::m_com
btVector3 m_com
Definition: btSoftBody.h:452
btSoftBody::Pose::m_pos
tVector3Array m_pos
Definition: btSoftBody.h:450
btSoftBody::Pose::m_aqq
btMatrix3x3 m_aqq
Definition: btSoftBody.h:455
btSoftBody::Pose::m_rot
btMatrix3x3 m_rot
Definition: btSoftBody.h:453
btSoftBody::Pose::m_wgh
tScalarArray m_wgh
Definition: btSoftBody.h:451
btSoftBody::RContact::m_c0
btMatrix3x3 m_c0
Definition: btSoftBody.h:345
btSoftBody::RayFromToCaster
RayFromToCaster takes a ray from, ray to (instead of direction!)
Definition: btSoftBody.h:762
btSoftBody::RayFromToCaster::RayFromToCaster
RayFromToCaster(const btVector3 &rayFrom, const btVector3 &rayTo, btScalar mxt)
Definition: btSoftBody.cpp:2355
btSoftBody::RayFromToCaster::Process
void Process(const btDbvtNode *leaf)
Definition: btSoftBody.cpp:2366
btSoftBody::RayFromToCaster::rayFromToTriangle
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)
Definition: btSoftBody.cpp:2383
btSoftBody::RayFromToCaster::m_rayNormalizedDirection
btVector3 m_rayNormalizedDirection
Definition: btSoftBody.h:765
btSoftBody::SContact::m_cfm
btScalar m_cfm[2]
Definition: btSoftBody.h:422
btSoftBody::TetraScratch::m_corotation
btMatrix3x3 m_corotation
Definition: btSoftBody.h:337
btSoftBody::Tetra::m_element_measure
btScalar m_element_measure
Definition: btSoftBody.h:326
btSoftBody::Tetra::m_Dm_inverse
btMatrix3x3 m_Dm_inverse
Definition: btSoftBody.h:324
btSoftBody::Tetra::m_F
btMatrix3x3 m_F
Definition: btSoftBody.h:325
btSoftBody::Tetra::m_P_inv
btVector4 m_P_inv[3]
Definition: btSoftBody.h:327
btSoftBody::eAeroModel::V_TwoSided
@ V_TwoSided
Vertex normals are oriented toward velocity.
Definition: btSoftBody.h:92
btSoftBody::eAeroModel::V_OneSided
@ V_OneSided
Vertex normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:94
btSoftBody::eAeroModel::V_TwoSidedLiftDrag
@ V_TwoSidedLiftDrag
Vertex normals are flipped to match velocity.
Definition: btSoftBody.h:93
btSoftBody::eAeroModel::F_OneSided
@ F_OneSided
Face normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:97
btSoftBody::eAeroModel::F_TwoSided
@ F_TwoSided
Vertex normals are taken as it is.
Definition: btSoftBody.h:95
btSoftBody::eAeroModel::F_TwoSidedLiftDrag
@ F_TwoSidedLiftDrag
Face normals are flipped to match velocity.
Definition: btSoftBody.h:96
btSoftBody::ePSolver::RContacts
@ RContacts
Anchor solver.
Definition: btSoftBody.h:119
btSoftBody::ePSolver::SContacts
@ SContacts
Rigid contacts solver.
Definition: btSoftBody.h:120
btSoftBody::ePSolver::Anchors
@ Anchors
Linear solver.
Definition: btSoftBody.h:118
btSoftBody::fCollision::SDF_RDN
@ SDF_RDN
GJK based Multibody vs. deformable face.
Definition: btSoftBody.h:177
btSoftBody::fCollision::VF_SS
@ VF_SS
Rigid versus soft mask.
Definition: btSoftBody.h:169
btSoftBody::fCollision::Default
@ Default
SDF based Rigid vs. deformable node.
Definition: btSoftBody.h:179
btSoftBody::fCollision::VF_DD
@ VF_DD
Cluster soft body self collision.
Definition: btSoftBody.h:172
btSoftBody::fCollision::CL_SS
@ CL_SS
Vertex vs face soft vs soft handling.
Definition: btSoftBody.h:170
btSoftBody::fCollision::CL_SELF
@ CL_SELF
Cluster vs cluster soft vs soft handling.
Definition: btSoftBody.h:171
btSoftBody::fCollision::SVSmask
@ SVSmask
rigid vs deformable
Definition: btSoftBody.h:168
btSoftBody::fCollision::SDF_RS
@ SDF_RS
Rigid versus soft mask.
Definition: btSoftBody.h:164
btSoftBody::fCollision::SDF_RD
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:166
btSoftBody::fCollision::SDF_RDF
@ SDF_RDF
Rigid versus deformable face mask.
Definition: btSoftBody.h:175
btSoftBody::fCollision::SDF_MDF
@ SDF_MDF
GJK based Rigid vs. deformable face.
Definition: btSoftBody.h:176
btSoftBody::fCollision::CL_RS
@ CL_RS
SDF based rigid vs soft.
Definition: btSoftBody.h:165
btSoftBody::fMaterial::Default
@ Default
Enable debug draw.
Definition: btSoftBody.h:191
btSoftBody::sCti::m_colObj
const btCollisionObject * m_colObj
Definition: btSoftBody.h:226
btSoftBody::sCti::m_offset
btScalar m_offset
Definition: btSoftBody.h:228
btSoftBody::sCti::m_normal
btVector3 m_normal
Definition: btSoftBody.h:227
btSoftBody::sMedium::m_velocity
btVector3 m_velocity
Definition: btSoftBody.h:235
btSoftBody::sRayCast::feature
eFeature::_ feature
soft body
Definition: btSoftBody.h:204
btSoftBody::sRayCast::fraction
btScalar fraction
feature index
Definition: btSoftBody.h:206
btSoftBody::sRayCast::index
int index
feature type
Definition: btSoftBody.h:205
btSoftBody::sRayCast::body
btSoftBody * body
Definition: btSoftBody.h:203
btSoftColliders::CollideCL_RS
Definition: btSoftBodyInternals.h:1490
btSoftColliders::CollideCL_RS::ProcessColObj
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
Definition: btSoftBodyInternals.h:1529
btSoftColliders::CollideCL_SS
Definition: btSoftBodyInternals.h:1552
btSoftColliders::CollideCL_SS::ProcessSoftSoft
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
Definition: btSoftBodyInternals.h:1592
btSoftColliders::CollideFF_DD
Definition: btSoftBodyInternals.h:1977
btSoftColliders::CollideFF_DD::useFaceNormal
bool useFaceNormal
Definition: btSoftBodyInternals.h:2045
btSoftColliders::CollideFF_DD::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:2044
btSoftColliders::CollideFF_DD::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:2043
btSoftColliders::CollideSDF_RDF
Definition: btSoftBodyInternals.h:1768
btSoftColliders::CollideSDF_RDF::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1861
btSoftColliders::CollideSDF_RDF::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1863
btSoftColliders::CollideSDF_RDF::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1865
btSoftColliders::CollideSDF_RDF::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1862
btSoftColliders::CollideSDF_RDF::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1864
btSoftColliders::CollideSDF_RD
Definition: btSoftBodyInternals.h:1659
btSoftColliders::CollideSDF_RD::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1761
btSoftColliders::CollideSDF_RD::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1759
btSoftColliders::CollideSDF_RD::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1757
btSoftColliders::CollideSDF_RD::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1758
btSoftColliders::CollideSDF_RD::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1760
btSoftColliders::CollideSDF_RS
Definition: btSoftBodyInternals.h:1607
btSoftColliders::CollideSDF_RS::dynmargin
btScalar dynmargin
Definition: btSoftBodyInternals.h:1651
btSoftColliders::CollideSDF_RS::m_rigidBody
btRigidBody * m_rigidBody
Definition: btSoftBodyInternals.h:1650
btSoftColliders::CollideSDF_RS::psb
btSoftBody * psb
Definition: btSoftBodyInternals.h:1648
btSoftColliders::CollideSDF_RS::stamargin
btScalar stamargin
Definition: btSoftBodyInternals.h:1652
btSoftColliders::CollideSDF_RS::m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
Definition: btSoftBodyInternals.h:1649
btSoftColliders::CollideVF_DD
Definition: btSoftBodyInternals.h:1928
btSoftColliders::CollideVF_DD::useFaceNormal
bool useFaceNormal
Definition: btSoftBodyInternals.h:1970
btSoftColliders::CollideVF_DD::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:1969
btSoftColliders::CollideVF_DD::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:1968
btSoftColliders::CollideVF_SS
Definition: btSoftBodyInternals.h:1872
btSoftColliders::CollideVF_SS::psb
btSoftBody * psb[2]
Definition: btSoftBodyInternals.h:1920
btSoftColliders::CollideVF_SS::mrg
btScalar mrg
Definition: btSoftBodyInternals.h:1921
btSparseSdf::Evaluate
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:196
Generated by doxygen 1.9.4