Bullet Collision Detection & Physics Library
btSoftBody.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
16 
17 #include "btSoftBodyInternals.h"
18 #include "BulletSoftBody/btSoftBodySolvers.h"
19 #include "btSoftBodyData.h"
20 #include "LinearMath/btSerializer.h"
21 
22 
23 //
24 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
25 :m_softBodySolver(0),m_worldInfo(worldInfo)
26 {
27  /* Init */
28  initDefaults();
29 
30  /* Default material */
31  Material* pm=appendMaterial();
32  pm->m_kLST = 1;
33  pm->m_kAST = 1;
34  pm->m_kVST = 1;
35  pm->m_flags = fMaterial::Default;
36 
37  /* Nodes */
38  const btScalar margin=getCollisionShape()->getMargin();
39  m_nodes.resize(node_count);
40  for(int i=0,ni=node_count;i<ni;++i)
41  {
42  Node& n=m_nodes[i];
43  ZeroInitialize(n);
44  n.m_x = x?*x++:btVector3(0,0,0);
45  n.m_q = n.m_x;
46  n.m_im = m?*m++:1;
47  n.m_im = n.m_im>0?1/n.m_im:0;
48  n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
49  n.m_material= pm;
50  }
51  updateBounds();
52 
53 }
54 
55 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
56 :m_worldInfo(worldInfo)
57 {
58  initDefaults();
59 }
60 
61 
62 void btSoftBody::initDefaults()
63 {
64  m_internalType = CO_SOFT_BODY;
65  m_cfg.aeromodel = eAeroModel::V_Point;
66  m_cfg.kVCF = 1;
67  m_cfg.kDG = 0;
68  m_cfg.kLF = 0;
69  m_cfg.kDP = 0;
70  m_cfg.kPR = 0;
71  m_cfg.kVC = 0;
72  m_cfg.kDF = (btScalar)0.2;
73  m_cfg.kMT = 0;
74  m_cfg.kCHR = (btScalar)1.0;
75  m_cfg.kKHR = (btScalar)0.1;
76  m_cfg.kSHR = (btScalar)1.0;
77  m_cfg.kAHR = (btScalar)0.7;
78  m_cfg.kSRHR_CL = (btScalar)0.1;
79  m_cfg.kSKHR_CL = (btScalar)1;
80  m_cfg.kSSHR_CL = (btScalar)0.5;
81  m_cfg.kSR_SPLT_CL = (btScalar)0.5;
82  m_cfg.kSK_SPLT_CL = (btScalar)0.5;
83  m_cfg.kSS_SPLT_CL = (btScalar)0.5;
84  m_cfg.maxvolume = (btScalar)1;
85  m_cfg.timescale = 1;
86  m_cfg.viterations = 0;
87  m_cfg.piterations = 1;
88  m_cfg.diterations = 0;
89  m_cfg.citerations = 4;
90  m_cfg.collisions = fCollision::Default;
91  m_pose.m_bvolume = false;
92  m_pose.m_bframe = false;
93  m_pose.m_volume = 0;
94  m_pose.m_com = btVector3(0,0,0);
95  m_pose.m_rot.setIdentity();
96  m_pose.m_scl.setIdentity();
97  m_tag = 0;
98  m_timeacc = 0;
99  m_bUpdateRtCst = true;
100  m_bounds[0] = btVector3(0,0,0);
101  m_bounds[1] = btVector3(0,0,0);
102  m_worldTransform.setIdentity();
103  setSolver(eSolverPresets::Positions);
104 
105  /* Collision shape */
107  m_collisionShape = new btSoftBodyCollisionShape(this);
108  m_collisionShape->setMargin(0.25f);
109 
110  m_initialWorldTransform.setIdentity();
111 
112  m_windVelocity = btVector3(0,0,0);
113  m_restLengthScale = btScalar(1.0);
114 }
115 
116 //
117 btSoftBody::~btSoftBody()
118 {
119  //for now, delete the internal shape
120  delete m_collisionShape;
121  int i;
122 
123  releaseClusters();
124  for(i=0;i<m_materials.size();++i)
125  btAlignedFree(m_materials[i]);
126  for(i=0;i<m_joints.size();++i)
127  btAlignedFree(m_joints[i]);
128 }
129 
130 //
131 bool btSoftBody::checkLink(int node0,int node1) const
132 {
133  return(checkLink(&m_nodes[node0],&m_nodes[node1]));
134 }
135 
136 //
137 bool btSoftBody::checkLink(const Node* node0,const Node* node1) const
138 {
139  const Node* n[]={node0,node1};
140  for(int i=0,ni=m_links.size();i<ni;++i)
141  {
142  const Link& l=m_links[i];
143  if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
144  (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
145  {
146  return(true);
147  }
148  }
149  return(false);
150 }
151 
152 //
153 bool btSoftBody::checkFace(int node0,int node1,int node2) const
154 {
155  const Node* n[]={ &m_nodes[node0],
156  &m_nodes[node1],
157  &m_nodes[node2]};
158  for(int i=0,ni=m_faces.size();i<ni;++i)
159  {
160  const Face& f=m_faces[i];
161  int c=0;
162  for(int j=0;j<3;++j)
163  {
164  if( (f.m_n[j]==n[0])||
165  (f.m_n[j]==n[1])||
166  (f.m_n[j]==n[2])) c|=1<<j; else break;
167  }
168  if(c==7) return(true);
169  }
170  return(false);
171 }
172 
173 //
174 btSoftBody::Material* btSoftBody::appendMaterial()
175 {
176  Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
177  if(m_materials.size()>0)
178  *pm=*m_materials[0];
179  else
180  ZeroInitialize(*pm);
181  m_materials.push_back(pm);
182  return(pm);
183 }
184 
185 //
186 void btSoftBody::appendNote( const char* text,
187  const btVector3& o,
188  const btVector4& c,
189  Node* n0,
190  Node* n1,
191  Node* n2,
192  Node* n3)
193 {
194  Note n;
195  ZeroInitialize(n);
196  n.m_rank = 0;
197  n.m_text = text;
198  n.m_offset = o;
199  n.m_coords[0] = c.x();
200  n.m_coords[1] = c.y();
201  n.m_coords[2] = c.z();
202  n.m_coords[3] = c.w();
203  n.m_nodes[0] = n0;n.m_rank+=n0?1:0;
204  n.m_nodes[1] = n1;n.m_rank+=n1?1:0;
205  n.m_nodes[2] = n2;n.m_rank+=n2?1:0;
206  n.m_nodes[3] = n3;n.m_rank+=n3?1:0;
207  m_notes.push_back(n);
208 }
209 
210 //
211 void btSoftBody::appendNote( const char* text,
212  const btVector3& o,
213  Node* feature)
214 {
215  appendNote(text,o,btVector4(1,0,0,0),feature);
216 }
217 
218 //
219 void btSoftBody::appendNote( const char* text,
220  const btVector3& o,
221  Link* feature)
222 {
223  static const btScalar w=1/(btScalar)2;
224  appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0],
225  feature->m_n[1]);
226 }
227 
228 //
229 void btSoftBody::appendNote( const char* text,
230  const btVector3& o,
231  Face* feature)
232 {
233  static const btScalar w=1/(btScalar)3;
234  appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
235  feature->m_n[1],
236  feature->m_n[2]);
237 }
238 
239 //
240 void btSoftBody::appendNode( const btVector3& x,btScalar m)
241 {
242  if(m_nodes.capacity()==m_nodes.size())
243  {
244  pointersToIndices();
245  m_nodes.reserve(m_nodes.size()*2+1);
246  indicesToPointers();
247  }
248  const btScalar margin=getCollisionShape()->getMargin();
249  m_nodes.push_back(Node());
250  Node& n=m_nodes[m_nodes.size()-1];
251  ZeroInitialize(n);
252  n.m_x = x;
253  n.m_q = n.m_x;
254  n.m_im = m>0?1/m:0;
255  n.m_material = m_materials[0];
256  n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
257 }
258 
259 //
260 void btSoftBody::appendLink(int model,Material* mat)
261 {
262  Link l;
263  if(model>=0)
264  l=m_links[model];
265  else
266  { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
267  m_links.push_back(l);
268 }
269 
270 //
271 void btSoftBody::appendLink( int node0,
272  int node1,
273  Material* mat,
274  bool bcheckexist)
275 {
276  appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
277 }
278 
279 //
280 void btSoftBody::appendLink( Node* node0,
281  Node* node1,
282  Material* mat,
283  bool bcheckexist)
284 {
285  if((!bcheckexist)||(!checkLink(node0,node1)))
286  {
287  appendLink(-1,mat);
288  Link& l=m_links[m_links.size()-1];
289  l.m_n[0] = node0;
290  l.m_n[1] = node1;
291  l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
292  m_bUpdateRtCst=true;
293  }
294 }
295 
296 //
297 void btSoftBody::appendFace(int model,Material* mat)
298 {
299  Face f;
300  if(model>=0)
301  { f=m_faces[model]; }
302  else
303  { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
304  m_faces.push_back(f);
305 }
306 
307 //
308 void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
309 {
310  if (node0==node1)
311  return;
312  if (node1==node2)
313  return;
314  if (node2==node0)
315  return;
316 
317  appendFace(-1,mat);
318  Face& f=m_faces[m_faces.size()-1];
319  btAssert(node0!=node1);
320  btAssert(node1!=node2);
321  btAssert(node2!=node0);
322  f.m_n[0] = &m_nodes[node0];
323  f.m_n[1] = &m_nodes[node1];
324  f.m_n[2] = &m_nodes[node2];
325  f.m_ra = AreaOf( f.m_n[0]->m_x,
326  f.m_n[1]->m_x,
327  f.m_n[2]->m_x);
328  m_bUpdateRtCst=true;
329 }
330 
331 //
332 void btSoftBody::appendTetra(int model,Material* mat)
333 {
334 Tetra t;
335 if(model>=0)
336  t=m_tetras[model];
337  else
338  { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
339 m_tetras.push_back(t);
340 }
341 
342 //
343 void btSoftBody::appendTetra(int node0,
344  int node1,
345  int node2,
346  int node3,
347  Material* mat)
348 {
349  appendTetra(-1,mat);
350  Tetra& t=m_tetras[m_tetras.size()-1];
351  t.m_n[0] = &m_nodes[node0];
352  t.m_n[1] = &m_nodes[node1];
353  t.m_n[2] = &m_nodes[node2];
354  t.m_n[3] = &m_nodes[node3];
355  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);
356  m_bUpdateRtCst=true;
357 }
358 
359 //
360 
361 void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence)
362 {
363  btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x;
364  appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence);
365 }
366 
367 //
368 void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence)
369 {
370  if (disableCollisionBetweenLinkedBodies)
371  {
372  if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
373  {
374  m_collisionDisabledObjects.push_back(body);
375  }
376  }
377 
378  Anchor a;
379  a.m_node = &m_nodes[node];
380  a.m_body = body;
381  a.m_local = localPivot;
382  a.m_node->m_battach = 1;
383  a.m_influence = influence;
384  m_anchors.push_back(a);
385 }
386 
387 //
388 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
389 {
390  LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
391  pj->m_bodies[0] = body0;
392  pj->m_bodies[1] = body1;
393  pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position;
394  pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position;
395  pj->m_cfm = specs.cfm;
396  pj->m_erp = specs.erp;
397  pj->m_split = specs.split;
398  m_joints.push_back(pj);
399 }
400 
401 //
402 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
403 {
404  appendLinearJoint(specs,m_clusters[0],body);
405 }
406 
407 //
408 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
409 {
410  appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
411 }
412 
413 //
414 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
415 {
416  AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
417  pj->m_bodies[0] = body0;
418  pj->m_bodies[1] = body1;
419  pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
420  pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
421  pj->m_cfm = specs.cfm;
422  pj->m_erp = specs.erp;
423  pj->m_split = specs.split;
424  pj->m_icontrol = specs.icontrol;
425  m_joints.push_back(pj);
426 }
427 
428 //
429 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
430 {
431  appendAngularJoint(specs,m_clusters[0],body);
432 }
433 
434 //
435 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
436 {
437  appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
438 }
439 
440 //
441 void btSoftBody::addForce(const btVector3& force)
442 {
443  for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
444 }
445 
446 //
447 void btSoftBody::addForce(const btVector3& force,int node)
448 {
449  Node& n=m_nodes[node];
450  if(n.m_im>0)
451  {
452  n.m_f += force;
453  }
454 }
455 
456 void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex)
457 {
458  btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
459 
460  const btScalar dt = m_sst.sdt;
461  const btScalar kLF = m_cfg.kLF;
462  const btScalar kDG = m_cfg.kDG;
463  //const btScalar kPR = m_cfg.kPR;
464  //const btScalar kVC = m_cfg.kVC;
465  const bool as_lift = kLF>0;
466  const bool as_drag = kDG>0;
467  const bool as_aero = as_lift || as_drag;
468  const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
469 
470  Node& n = m_nodes[nodeIndex];
471 
472  if( n.m_im>0 )
473  {
474  btSoftBody::sMedium medium;
475 
476  EvaluateMedium(m_worldInfo, n.m_x, medium);
477  medium.m_velocity = windVelocity;
478  medium.m_density = m_worldInfo->air_density;
479 
480  /* Aerodynamics */
481  if(as_vaero)
482  {
483  const btVector3 rel_v = n.m_v - medium.m_velocity;
484  const btScalar rel_v_len = rel_v.length();
485  const btScalar rel_v2 = rel_v.length2();
486 
487  if(rel_v2>SIMD_EPSILON)
488  {
489  const btVector3 rel_v_nrm = rel_v.normalized();
490  btVector3 nrm = n.m_n;
491 
492  if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag)
493  {
494  nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
495  btVector3 fDrag(0, 0, 0);
496  btVector3 fLift(0, 0, 0);
497 
498  btScalar n_dot_v = nrm.dot(rel_v_nrm);
499  btScalar tri_area = 0.5f * n.m_area;
500 
501  fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
502 
503  // Check angle of attack
504  // cos(10º) = 0.98480
505  if ( 0 < n_dot_v && n_dot_v < 0.98480f)
506  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));
507 
508  // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
509  btVector3 del_v_by_fDrag = fDrag*n.m_im*m_sst.sdt;
510  btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
511  btScalar v_len2 = n.m_v.length2();
512 
513  if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
514  {
515  btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
516  btScalar v_len = n.m_v.length();
517  fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
518  }
519 
520  n.m_f += fDrag;
521  n.m_f += fLift;
522  }
523  else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided)
524  {
525  if (btSoftBody::eAeroModel::V_TwoSided)
526  nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
527 
528  const btScalar dvn = btDot(rel_v,nrm);
529  /* Compute forces */
530  if(dvn>0)
531  {
532  btVector3 force(0,0,0);
533  const btScalar c0 = n.m_area * dvn * rel_v2/2;
534  const btScalar c1 = c0 * medium.m_density;
535  force += nrm*(-c1*kLF);
536  force += rel_v.normalized() * (-c1 * kDG);
537  ApplyClampedForce(n, force, dt);
538  }
539  }
540  }
541  }
542  }
543 }
544 
545 void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex)
546 {
547  const btScalar dt = m_sst.sdt;
548  const btScalar kLF = m_cfg.kLF;
549  const btScalar kDG = m_cfg.kDG;
550 // const btScalar kPR = m_cfg.kPR;
551 // const btScalar kVC = m_cfg.kVC;
552  const bool as_lift = kLF>0;
553  const bool as_drag = kDG>0;
554  const bool as_aero = as_lift || as_drag;
555  const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
556 
557  if(as_faero)
558  {
559  btSoftBody::Face& f=m_faces[faceIndex];
560 
561  btSoftBody::sMedium medium;
562 
563  const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
564  const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
565  EvaluateMedium(m_worldInfo,x,medium);
566  medium.m_velocity = windVelocity;
567  medium.m_density = m_worldInfo->air_density;
568  const btVector3 rel_v=v-medium.m_velocity;
569  const btScalar rel_v_len = rel_v.length();
570  const btScalar rel_v2=rel_v.length2();
571 
572  if(rel_v2>SIMD_EPSILON)
573  {
574  const btVector3 rel_v_nrm = rel_v.normalized();
575  btVector3 nrm = f.m_normal;
576 
577  if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag)
578  {
579  nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
580 
581  btVector3 fDrag(0, 0, 0);
582  btVector3 fLift(0, 0, 0);
583 
584  btScalar n_dot_v = nrm.dot(rel_v_nrm);
585  btScalar tri_area = 0.5f * f.m_ra;
586 
587  fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
588 
589  // Check angle of attack
590  // cos(10º) = 0.98480
591  if ( 0 < n_dot_v && n_dot_v < 0.98480f)
592  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));
593 
594  fDrag /= 3;
595  fLift /= 3;
596 
597  for(int j=0;j<3;++j)
598  {
599  if (f.m_n[j]->m_im>0)
600  {
601  // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
602  btVector3 del_v_by_fDrag = fDrag*f.m_n[j]->m_im*m_sst.sdt;
603  btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
604  btScalar v_len2 = f.m_n[j]->m_v.length2();
605 
606  if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
607  {
608  btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
609  btScalar v_len = f.m_n[j]->m_v.length();
610  fDrag *= btScalar(0.8)*(v_len / del_v_by_fDrag_len);
611  }
612 
613  f.m_n[j]->m_f += fDrag;
614  f.m_n[j]->m_f += fLift;
615  }
616  }
617  }
618  else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided)
619  {
620  if (btSoftBody::eAeroModel::F_TwoSided)
621  nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
622 
623  const btScalar dvn=btDot(rel_v,nrm);
624  /* Compute forces */
625  if(dvn>0)
626  {
627  btVector3 force(0,0,0);
628  const btScalar c0 = f.m_ra*dvn*rel_v2;
629  const btScalar c1 = c0*medium.m_density;
630  force += nrm*(-c1*kLF);
631  force += rel_v.normalized()*(-c1*kDG);
632  force /= 3;
633  for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
634  }
635  }
636  }
637  }
638 
639 }
640 
641 //
642 void btSoftBody::addVelocity(const btVector3& velocity)
643 {
644  for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
645 }
646 
647 /* Set velocity for the entire body */
648 void btSoftBody::setVelocity( const btVector3& velocity)
649 {
650  for(int i=0,ni=m_nodes.size();i<ni;++i)
651  {
652  Node& n=m_nodes[i];
653  if(n.m_im>0)
654  {
655  n.m_v = velocity;
656  }
657  }
658 }
659 
660 
661 //
662 void btSoftBody::addVelocity(const btVector3& velocity,int node)
663 {
664  Node& n=m_nodes[node];
665  if(n.m_im>0)
666  {
667  n.m_v += velocity;
668  }
669 }
670 
671 //
672 void btSoftBody::setMass(int node,btScalar mass)
673 {
674  m_nodes[node].m_im=mass>0?1/mass:0;
675  m_bUpdateRtCst=true;
676 }
677 
678 //
679 btScalar btSoftBody::getMass(int node) const
680 {
681  return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
682 }
683 
684 //
685 btScalar btSoftBody::getTotalMass() const
686 {
687  btScalar mass=0;
688  for(int i=0;i<m_nodes.size();++i)
689  {
690  mass+=getMass(i);
691  }
692  return(mass);
693 }
694 
695 //
696 void btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
697 {
698  int i;
699 
700  if(fromfaces)
701  {
702 
703  for(i=0;i<m_nodes.size();++i)
704  {
705  m_nodes[i].m_im=0;
706  }
707  for(i=0;i<m_faces.size();++i)
708  {
709  const Face& f=m_faces[i];
710  const btScalar twicearea=AreaOf( f.m_n[0]->m_x,
711  f.m_n[1]->m_x,
712  f.m_n[2]->m_x);
713  for(int j=0;j<3;++j)
714  {
715  f.m_n[j]->m_im+=twicearea;
716  }
717  }
718  for( i=0;i<m_nodes.size();++i)
719  {
720  m_nodes[i].m_im=1/m_nodes[i].m_im;
721  }
722  }
723  const btScalar tm=getTotalMass();
724  const btScalar itm=1/tm;
725  for( i=0;i<m_nodes.size();++i)
726  {
727  m_nodes[i].m_im/=itm*mass;
728  }
729  m_bUpdateRtCst=true;
730 }
731 
732 //
733 void btSoftBody::setTotalDensity(btScalar density)
734 {
735  setTotalMass(getVolume()*density,true);
736 }
737 
738 //
739 void btSoftBody::setVolumeMass(btScalar mass)
740 {
741 btAlignedObjectArray<btScalar> ranks;
742 ranks.resize(m_nodes.size(),0);
743 int i;
744 
745 for(i=0;i<m_nodes.size();++i)
746  {
747  m_nodes[i].m_im=0;
748  }
749 for(i=0;i<m_tetras.size();++i)
750  {
751  const Tetra& t=m_tetras[i];
752  for(int j=0;j<4;++j)
753  {
754  t.m_n[j]->m_im+=btFabs(t.m_rv);
755  ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
756  }
757  }
758 for( i=0;i<m_nodes.size();++i)
759  {
760  if(m_nodes[i].m_im>0)
761  {
762  m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
763  }
764  }
765 setTotalMass(mass,false);
766 }
767 
768 //
769 void btSoftBody::setVolumeDensity(btScalar density)
770 {
771 btScalar volume=0;
772 for(int i=0;i<m_tetras.size();++i)
773  {
774  const Tetra& t=m_tetras[i];
775  for(int j=0;j<4;++j)
776  {
777  volume+=btFabs(t.m_rv);
778  }
779  }
780 setVolumeMass(volume*density/6);
781 }
782 
783 //
784 void btSoftBody::transform(const btTransform& trs)
785 {
786  const btScalar margin=getCollisionShape()->getMargin();
787  ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
788 
789  for(int i=0,ni=m_nodes.size();i<ni;++i)
790  {
791  Node& n=m_nodes[i];
792  n.m_x=trs*n.m_x;
793  n.m_q=trs*n.m_q;
794  n.m_n=trs.getBasis()*n.m_n;
795  vol = btDbvtVolume::FromCR(n.m_x,margin);
796 
797  m_ndbvt.update(n.m_leaf,vol);
798  }
799  updateNormals();
800  updateBounds();
801  updateConstants();
802  m_initialWorldTransform = trs;
803 }
804 
805 //
806 void btSoftBody::translate(const btVector3& trs)
807 {
808  btTransform t;
809  t.setIdentity();
810  t.setOrigin(trs);
811  transform(t);
812 }
813 
814 //
815 void btSoftBody::rotate( const btQuaternion& rot)
816 {
817  btTransform t;
818  t.setIdentity();
819  t.setRotation(rot);
820  transform(t);
821 }
822 
823 //
824 void btSoftBody::scale(const btVector3& scl)
825 {
826 
827  const btScalar margin=getCollisionShape()->getMargin();
828  ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
829 
830  for(int i=0,ni=m_nodes.size();i<ni;++i)
831  {
832  Node& n=m_nodes[i];
833  n.m_x*=scl;
834  n.m_q*=scl;
835  vol = btDbvtVolume::FromCR(n.m_x,margin);
836  m_ndbvt.update(n.m_leaf,vol);
837  }
838  updateNormals();
839  updateBounds();
840  updateConstants();
841 }
842 
843 //
844 btScalar btSoftBody::getRestLengthScale()
845 {
846  return m_restLengthScale;
847 }
848 
849 //
850 void btSoftBody::setRestLengthScale(btScalar restLengthScale)
851 {
852  for(int i=0, ni=m_links.size(); i<ni; ++i)
853  {
854  Link& l=m_links[i];
855  l.m_rl = l.m_rl / m_restLengthScale * restLengthScale;
856  l.m_c1 = l.m_rl*l.m_rl;
857  }
858  m_restLengthScale = restLengthScale;
859 
860  if (getActivationState() == ISLAND_SLEEPING)
861  activate();
862 }
863 
864 //
865 void btSoftBody::setPose(bool bvolume,bool bframe)
866 {
867  m_pose.m_bvolume = bvolume;
868  m_pose.m_bframe = bframe;
869  int i,ni;
870 
871  /* Weights */
872  const btScalar omass=getTotalMass();
873  const btScalar kmass=omass*m_nodes.size()*1000;
874  btScalar tmass=omass;
875  m_pose.m_wgh.resize(m_nodes.size());
876  for(i=0,ni=m_nodes.size();i<ni;++i)
877  {
878  if(m_nodes[i].m_im<=0) tmass+=kmass;
879  }
880  for( i=0,ni=m_nodes.size();i<ni;++i)
881  {
882  Node& n=m_nodes[i];
883  m_pose.m_wgh[i]= n.m_im>0 ?
884  1/(m_nodes[i].m_im*tmass) :
885  kmass/tmass;
886  }
887  /* Pos */
888  const btVector3 com=evaluateCom();
889  m_pose.m_pos.resize(m_nodes.size());
890  for( i=0,ni=m_nodes.size();i<ni;++i)
891  {
892  m_pose.m_pos[i]=m_nodes[i].m_x-com;
893  }
894  m_pose.m_volume = bvolume?getVolume():0;
895  m_pose.m_com = com;
896  m_pose.m_rot.setIdentity();
897  m_pose.m_scl.setIdentity();
898  /* Aqq */
899  m_pose.m_aqq[0] =
900  m_pose.m_aqq[1] =
901  m_pose.m_aqq[2] = btVector3(0,0,0);
902  for( i=0,ni=m_nodes.size();i<ni;++i)
903  {
904  const btVector3& q=m_pose.m_pos[i];
905  const btVector3 mq=m_pose.m_wgh[i]*q;
906  m_pose.m_aqq[0]+=mq.x()*q;
907  m_pose.m_aqq[1]+=mq.y()*q;
908  m_pose.m_aqq[2]+=mq.z()*q;
909  }
910  m_pose.m_aqq=m_pose.m_aqq.inverse();
911 
912  updateConstants();
913 }
914 
915 void btSoftBody::resetLinkRestLengths()
916 {
917  for(int i=0, ni=m_links.size();i<ni;++i)
918  {
919  Link& l = m_links[i];
920  l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
921  l.m_c1 = l.m_rl*l.m_rl;
922  }
923 }
924 
925 //
926 btScalar btSoftBody::getVolume() const
927 {
928  btScalar vol=0;
929  if(m_nodes.size()>0)
930  {
931  int i,ni;
932 
933  const btVector3 org=m_nodes[0].m_x;
934  for(i=0,ni=m_faces.size();i<ni;++i)
935  {
936  const Face& f=m_faces[i];
937  vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
938  }
939  vol/=(btScalar)6;
940  }
941  return(vol);
942 }
943 
944 //
945 int btSoftBody::clusterCount() const
946 {
947  return(m_clusters.size());
948 }
949 
950 //
951 btVector3 btSoftBody::clusterCom(const Cluster* cluster)
952 {
953  btVector3 com(0,0,0);
954  for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
955  {
956  com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
957  }
958  return(com*cluster->m_imass);
959 }
960 
961 //
962 btVector3 btSoftBody::clusterCom(int cluster) const
963 {
964  return(clusterCom(m_clusters[cluster]));
965 }
966 
967 //
968 btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
969 {
970  return(cluster->m_lv+btCross(cluster->m_av,rpos));
971 }
972 
973 //
974 void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
975 {
976  const btVector3 li=cluster->m_imass*impulse;
977  const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
978  cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
979  cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
980  cluster->m_nvimpulses++;
981 }
982 
983 //
984 void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
985 {
986  const btVector3 li=cluster->m_imass*impulse;
987  const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
988  cluster->m_dimpulses[0]+=li;
989  cluster->m_dimpulses[1]+=ai;
990  cluster->m_ndimpulses++;
991 }
992 
993 //
994 void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
995 {
996  if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity);
997  if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift);
998 }
999 
1000 //
1001 void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
1002 {
1003  const btVector3 ai=cluster->m_invwi*impulse;
1004  cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
1005  cluster->m_nvimpulses++;
1006 }
1007 
1008 //
1009 void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
1010 {
1011  const btVector3 ai=cluster->m_invwi*impulse;
1012  cluster->m_dimpulses[1]+=ai;
1013  cluster->m_ndimpulses++;
1014 }
1015 
1016 //
1017 void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
1018 {
1019  if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity);
1020  if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift);
1021 }
1022 
1023 //
1024 void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
1025 {
1026  cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
1027  cluster->m_ndimpulses++;
1028 }
1029 
1030 struct NodeLinks
1031 {
1032  btAlignedObjectArray<int> m_links;
1033 };
1034 
1035 
1036 
1037 //
1038 int btSoftBody::generateBendingConstraints(int distance,Material* mat)
1039 {
1040  int i,j;
1041 
1042  if(distance>1)
1043  {
1044  /* Build graph */
1045  const int n=m_nodes.size();
1046  const unsigned inf=(~(unsigned)0)>>1;
1047  unsigned* adj=new unsigned[n*n];
1048 
1049 
1050 #define IDX(_x_,_y_) ((_y_)*n+(_x_))
1051  for(j=0;j<n;++j)
1052  {
1053  for(i=0;i<n;++i)
1054  {
1055  if(i!=j)
1056  {
1057  adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
1058  }
1059  else
1060  {
1061  adj[IDX(i,j)]=adj[IDX(j,i)]=0;
1062  }
1063  }
1064  }
1065  for( i=0;i<m_links.size();++i)
1066  {
1067  const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
1068  const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
1069  adj[IDX(ia,ib)]=1;
1070  adj[IDX(ib,ia)]=1;
1071  }
1072 
1073 
1074  //special optimized case for distance == 2
1075  if (distance == 2)
1076  {
1077 
1078  btAlignedObjectArray<NodeLinks> nodeLinks;
1079 
1080 
1081  /* Build node links */
1082  nodeLinks.resize(m_nodes.size());
1083 
1084  for( i=0;i<m_links.size();++i)
1085  {
1086  const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
1087  const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
1088  if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
1089  nodeLinks[ia].m_links.push_back(ib);
1090 
1091  if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
1092  nodeLinks[ib].m_links.push_back(ia);
1093  }
1094  for (int ii=0;ii<nodeLinks.size();ii++)
1095  {
1096  int i=ii;
1097 
1098  for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
1099  {
1100  int k = nodeLinks[ii].m_links[jj];
1101  for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
1102  {
1103  int j = nodeLinks[k].m_links[kk];
1104  if (i!=j)
1105  {
1106  const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
1107  btAssert(sum==2);
1108  if(adj[IDX(i,j)]>sum)
1109  {
1110  adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
1111  }
1112  }
1113 
1114  }
1115  }
1116  }
1117  } else
1118  {
1120  for(int k=0;k<n;++k)
1121  {
1122  for(j=0;j<n;++j)
1123  {
1124  for(i=j+1;i<n;++i)
1125  {
1126  const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
1127  if(adj[IDX(i,j)]>sum)
1128  {
1129  adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
1130  }
1131  }
1132  }
1133  }
1134  }
1135 
1136 
1137  /* Build links */
1138  int nlinks=0;
1139  for(j=0;j<n;++j)
1140  {
1141  for(i=j+1;i<n;++i)
1142  {
1143  if(adj[IDX(i,j)]==(unsigned)distance)
1144  {
1145  appendLink(i,j,mat);
1146  m_links[m_links.size()-1].m_bbending=1;
1147  ++nlinks;
1148  }
1149  }
1150  }
1151  delete[] adj;
1152  return(nlinks);
1153  }
1154  return(0);
1155 }
1156 
1157 //
1158 void btSoftBody::randomizeConstraints()
1159 {
1160  unsigned long seed=243703;
1161 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
1162  int i,ni;
1163 
1164  for(i=0,ni=m_links.size();i<ni;++i)
1165  {
1166  btSwap(m_links[i],m_links[NEXTRAND%ni]);
1167  }
1168  for(i=0,ni=m_faces.size();i<ni;++i)
1169  {
1170  btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
1171  }
1172 #undef NEXTRAND
1173 }
1174 
1175 //
1176 void btSoftBody::releaseCluster(int index)
1177 {
1178  Cluster* c=m_clusters[index];
1179  if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
1180  c->~Cluster();
1181  btAlignedFree(c);
1182  m_clusters.remove(c);
1183 }
1184 
1185 //
1186 void btSoftBody::releaseClusters()
1187 {
1188  while(m_clusters.size()>0) releaseCluster(0);
1189 }
1190 
1191 //
1192 int btSoftBody::generateClusters(int k,int maxiterations)
1193 {
1194  int i;
1195  releaseClusters();
1196  m_clusters.resize(btMin(k,m_nodes.size()));
1197  for(i=0;i<m_clusters.size();++i)
1198  {
1199  m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1200  m_clusters[i]->m_collide= true;
1201  }
1202  k=m_clusters.size();
1203  if(k>0)
1204  {
1205  /* Initialize */
1206  btAlignedObjectArray<btVector3> centers;
1207  btVector3 cog(0,0,0);
1208  int i;
1209  for(i=0;i<m_nodes.size();++i)
1210  {
1211  cog+=m_nodes[i].m_x;
1212  m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
1213  }
1214  cog/=(btScalar)m_nodes.size();
1215  centers.resize(k,cog);
1216  /* Iterate */
1217  const btScalar slope=16;
1218  bool changed;
1219  int iterations=0;
1220  do {
1221  const btScalar w=2-btMin<btScalar>(1,iterations/slope);
1222  changed=false;
1223  iterations++;
1224  int i;
1225 
1226  for(i=0;i<k;++i)
1227  {
1228  btVector3 c(0,0,0);
1229  for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
1230  {
1231  c+=m_clusters[i]->m_nodes[j]->m_x;
1232  }
1233  if(m_clusters[i]->m_nodes.size())
1234  {
1235  c /= (btScalar)m_clusters[i]->m_nodes.size();
1236  c = centers[i]+(c-centers[i])*w;
1237  changed |= ((c-centers[i]).length2()>SIMD_EPSILON);
1238  centers[i] = c;
1239  m_clusters[i]->m_nodes.resize(0);
1240  }
1241  }
1242  for(i=0;i<m_nodes.size();++i)
1243  {
1244  const btVector3 nx=m_nodes[i].m_x;
1245  int kbest=0;
1246  btScalar kdist=ClusterMetric(centers[0],nx);
1247  for(int j=1;j<k;++j)
1248  {
1249  const btScalar d=ClusterMetric(centers[j],nx);
1250  if(d<kdist)
1251  {
1252  kbest=j;
1253  kdist=d;
1254  }
1255  }
1256  m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1257  }
1258  } while(changed&&(iterations<maxiterations));
1259  /* Merge */
1260  btAlignedObjectArray<int> cids;
1261  cids.resize(m_nodes.size(),-1);
1262  for(i=0;i<m_clusters.size();++i)
1263  {
1264  for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
1265  {
1266  cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
1267  }
1268  }
1269  for(i=0;i<m_faces.size();++i)
1270  {
1271  const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]),
1272  int(m_faces[i].m_n[1]-&m_nodes[0]),
1273  int(m_faces[i].m_n[2]-&m_nodes[0])};
1274  for(int j=0;j<3;++j)
1275  {
1276  const int cid=cids[idx[j]];
1277  for(int q=1;q<3;++q)
1278  {
1279  const int kid=idx[(j+q)%3];
1280  if(cids[kid]!=cid)
1281  {
1282  if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
1283  {
1284  m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1285  }
1286  }
1287  }
1288  }
1289  }
1290  /* Master */
1291  if(m_clusters.size()>1)
1292  {
1293  Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1294  pmaster->m_collide = false;
1295  pmaster->m_nodes.reserve(m_nodes.size());
1296  for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1297  m_clusters.push_back(pmaster);
1298  btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
1299  }
1300  /* Terminate */
1301  for(i=0;i<m_clusters.size();++i)
1302  {
1303  if(m_clusters[i]->m_nodes.size()==0)
1304  {
1305  releaseCluster(i--);
1306  }
1307  }
1308  } else
1309  {
1310  //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1311  if (m_tetras.size())
1312  {
1313  m_clusters.resize(m_tetras.size());
1314  for(i=0;i<m_clusters.size();++i)
1315  {
1316  m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1317  m_clusters[i]->m_collide= true;
1318  }
1319  for (i=0;i<m_tetras.size();i++)
1320  {
1321  for (int j=0;j<4;j++)
1322  {
1323  m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1324  }
1325  }
1326 
1327  } else
1328  {
1329  m_clusters.resize(m_faces.size());
1330  for(i=0;i<m_clusters.size();++i)
1331  {
1332  m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1333  m_clusters[i]->m_collide= true;
1334  }
1335 
1336  for(i=0;i<m_faces.size();++i)
1337  {
1338  for(int j=0;j<3;++j)
1339  {
1340  m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1341  }
1342  }
1343  }
1344  }
1345 
1346  if (m_clusters.size())
1347  {
1348  initializeClusters();
1349  updateClusters();
1350 
1351 
1352  //for self-collision
1353  m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
1354  {
1355  for (int c0=0;c0<m_clusters.size();c0++)
1356  {
1357  m_clusters[c0]->m_clusterIndex=c0;
1358  for (int c1=0;c1<m_clusters.size();c1++)
1359  {
1360 
1361  bool connected=false;
1362  Cluster* cla = m_clusters[c0];
1363  Cluster* clb = m_clusters[c1];
1364  for (int i=0;!connected&&i<cla->m_nodes.size();i++)
1365  {
1366  for (int j=0;j<clb->m_nodes.size();j++)
1367  {
1368  if (cla->m_nodes[i] == clb->m_nodes[j])
1369  {
1370  connected=true;
1371  break;
1372  }
1373  }
1374  }
1375  m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
1376  }
1377  }
1378  }
1379  }
1380 
1381  return(m_clusters.size());
1382 }
1383 
1384 //
1385 void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
1386 {
1387  const Node* nbase = &m_nodes[0];
1388  int ncount = m_nodes.size();
1389  btSymMatrix<int> edges(ncount,-2);
1390  int newnodes=0;
1391  int i,j,k,ni;
1392 
1393  /* Filter out */
1394  for(i=0;i<m_links.size();++i)
1395  {
1396  Link& l=m_links[i];
1397  if(l.m_bbending)
1398  {
1399  if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
1400  {
1401  btSwap(m_links[i],m_links[m_links.size()-1]);
1402  m_links.pop_back();--i;
1403  }
1404  }
1405  }
1406  /* Fill edges */
1407  for(i=0;i<m_links.size();++i)
1408  {
1409  Link& l=m_links[i];
1410  edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
1411  }
1412  for(i=0;i<m_faces.size();++i)
1413  {
1414  Face& f=m_faces[i];
1415  edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
1416  edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
1417  edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
1418  }
1419  /* Intersect */
1420  for(i=0;i<ncount;++i)
1421  {
1422  for(j=i+1;j<ncount;++j)
1423  {
1424  if(edges(i,j)==-1)
1425  {
1426  Node& a=m_nodes[i];
1427  Node& b=m_nodes[j];
1428  const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
1429  if(t>0)
1430  {
1431  const btVector3 x=Lerp(a.m_x,b.m_x,t);
1432  const btVector3 v=Lerp(a.m_v,b.m_v,t);
1433  btScalar m=0;
1434  if(a.m_im>0)
1435  {
1436  if(b.m_im>0)
1437  {
1438  const btScalar ma=1/a.m_im;
1439  const btScalar mb=1/b.m_im;
1440  const btScalar mc=Lerp(ma,mb,t);
1441  const btScalar f=(ma+mb)/(ma+mb+mc);
1442  a.m_im=1/(ma*f);
1443  b.m_im=1/(mb*f);
1444  m=mc*f;
1445  }
1446  else
1447  { a.m_im/=0.5f;m=1/a.m_im; }
1448  }
1449  else
1450  {
1451  if(b.m_im>0)
1452  { b.m_im/=0.5f;m=1/b.m_im; }
1453  else
1454  m=0;
1455  }
1456  appendNode(x,m);
1457  edges(i,j)=m_nodes.size()-1;
1458  m_nodes[edges(i,j)].m_v=v;
1459  ++newnodes;
1460  }
1461  }
1462  }
1463  }
1464  nbase=&m_nodes[0];
1465  /* Refine links */
1466  for(i=0,ni=m_links.size();i<ni;++i)
1467  {
1468  Link& feat=m_links[i];
1469  const int idx[]={ int(feat.m_n[0]-nbase),
1470  int(feat.m_n[1]-nbase)};
1471  if((idx[0]<ncount)&&(idx[1]<ncount))
1472  {
1473  const int ni=edges(idx[0],idx[1]);
1474  if(ni>0)
1475  {
1476  appendLink(i);
1477  Link* pft[]={ &m_links[i],
1478  &m_links[m_links.size()-1]};
1479  pft[0]->m_n[0]=&m_nodes[idx[0]];
1480  pft[0]->m_n[1]=&m_nodes[ni];
1481  pft[1]->m_n[0]=&m_nodes[ni];
1482  pft[1]->m_n[1]=&m_nodes[idx[1]];
1483  }
1484  }
1485  }
1486  /* Refine faces */
1487  for(i=0;i<m_faces.size();++i)
1488  {
1489  const Face& feat=m_faces[i];
1490  const int idx[]={ int(feat.m_n[0]-nbase),
1491  int(feat.m_n[1]-nbase),
1492  int(feat.m_n[2]-nbase)};
1493  for(j=2,k=0;k<3;j=k++)
1494  {
1495  if((idx[j]<ncount)&&(idx[k]<ncount))
1496  {
1497  const int ni=edges(idx[j],idx[k]);
1498  if(ni>0)
1499  {
1500  appendFace(i);
1501  const int l=(k+1)%3;
1502  Face* pft[]={ &m_faces[i],
1503  &m_faces[m_faces.size()-1]};
1504  pft[0]->m_n[0]=&m_nodes[idx[l]];
1505  pft[0]->m_n[1]=&m_nodes[idx[j]];
1506  pft[0]->m_n[2]=&m_nodes[ni];
1507  pft[1]->m_n[0]=&m_nodes[ni];
1508  pft[1]->m_n[1]=&m_nodes[idx[k]];
1509  pft[1]->m_n[2]=&m_nodes[idx[l]];
1510  appendLink(ni,idx[l],pft[0]->m_material);
1511  --i;break;
1512  }
1513  }
1514  }
1515  }
1516  /* Cut */
1517  if(cut)
1518  {
1519  btAlignedObjectArray<int> cnodes;
1520  const int pcount=ncount;
1521  int i;
1522  ncount=m_nodes.size();
1523  cnodes.resize(ncount,0);
1524  /* Nodes */
1525  for(i=0;i<ncount;++i)
1526  {
1527  const btVector3 x=m_nodes[i].m_x;
1528  if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
1529  {
1530  const btVector3 v=m_nodes[i].m_v;
1531  btScalar m=getMass(i);
1532  if(m>0) { m*=0.5f;m_nodes[i].m_im/=0.5f; }
1533  appendNode(x,m);
1534  cnodes[i]=m_nodes.size()-1;
1535  m_nodes[cnodes[i]].m_v=v;
1536  }
1537  }
1538  nbase=&m_nodes[0];
1539  /* Links */
1540  for(i=0,ni=m_links.size();i<ni;++i)
1541  {
1542  const int id[]={ int(m_links[i].m_n[0]-nbase),
1543  int(m_links[i].m_n[1]-nbase)};
1544  int todetach=0;
1545  if(cnodes[id[0]]&&cnodes[id[1]])
1546  {
1547  appendLink(i);
1548  todetach=m_links.size()-1;
1549  }
1550  else
1551  {
1552  if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
1553  (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
1554  todetach=i;
1555  }
1556  if(todetach)
1557  {
1558  Link& l=m_links[todetach];
1559  for(int j=0;j<2;++j)
1560  {
1561  int cn=cnodes[int(l.m_n[j]-nbase)];
1562  if(cn) l.m_n[j]=&m_nodes[cn];
1563  }
1564  }
1565  }
1566  /* Faces */
1567  for(i=0,ni=m_faces.size();i<ni;++i)
1568  {
1569  Node** n= m_faces[i].m_n;
1570  if( (ifn->Eval(n[0]->m_x)<accurary)&&
1571  (ifn->Eval(n[1]->m_x)<accurary)&&
1572  (ifn->Eval(n[2]->m_x)<accurary))
1573  {
1574  for(int j=0;j<3;++j)
1575  {
1576  int cn=cnodes[int(n[j]-nbase)];
1577  if(cn) n[j]=&m_nodes[cn];
1578  }
1579  }
1580  }
1581  /* Clean orphans */
1582  int nnodes=m_nodes.size();
1583  btAlignedObjectArray<int> ranks;
1584  btAlignedObjectArray<int> todelete;
1585  ranks.resize(nnodes,0);
1586  for(i=0,ni=m_links.size();i<ni;++i)
1587  {
1588  for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
1589  }
1590  for(i=0,ni=m_faces.size();i<ni;++i)
1591  {
1592  for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
1593  }
1594  for(i=0;i<m_links.size();++i)
1595  {
1596  const int id[]={ int(m_links[i].m_n[0]-nbase),
1597  int(m_links[i].m_n[1]-nbase)};
1598  const bool sg[]={ ranks[id[0]]==1,
1599  ranks[id[1]]==1};
1600  if(sg[0]||sg[1])
1601  {
1602  --ranks[id[0]];
1603  --ranks[id[1]];
1604  btSwap(m_links[i],m_links[m_links.size()-1]);
1605  m_links.pop_back();--i;
1606  }
1607  }
1608 #if 0
1609  for(i=nnodes-1;i>=0;--i)
1610  {
1611  if(!ranks[i]) todelete.push_back(i);
1612  }
1613  if(todelete.size())
1614  {
1615  btAlignedObjectArray<int>& map=ranks;
1616  for(int i=0;i<nnodes;++i) map[i]=i;
1617  PointersToIndices(this);
1618  for(int i=0,ni=todelete.size();i<ni;++i)
1619  {
1620  int j=todelete[i];
1621  int& a=map[j];
1622  int& b=map[--nnodes];
1623  m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1624  btSwap(m_nodes[a],m_nodes[b]);
1625  j=a;a=b;b=j;
1626  }
1627  IndicesToPointers(this,&map[0]);
1628  m_nodes.resize(nnodes);
1629  }
1630 #endif
1631  }
1632  m_bUpdateRtCst=true;
1633 }
1634 
1635 //
1636 bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
1637 {
1638  return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
1639 }
1640 
1641 //
1642 bool btSoftBody::cutLink(int node0,int node1,btScalar position)
1643 {
1644  bool done=false;
1645  int i,ni;
1646 // const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1647  const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
1648  const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
1649  const btScalar m=1;
1650  appendNode(x,m);
1651  appendNode(x,m);
1652  Node* pa=&m_nodes[node0];
1653  Node* pb=&m_nodes[node1];
1654  Node* pn[2]={ &m_nodes[m_nodes.size()-2],
1655  &m_nodes[m_nodes.size()-1]};
1656  pn[0]->m_v=v;
1657  pn[1]->m_v=v;
1658  for(i=0,ni=m_links.size();i<ni;++i)
1659  {
1660  const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
1661  if(mtch!=-1)
1662  {
1663  appendLink(i);
1664  Link* pft[]={&m_links[i],&m_links[m_links.size()-1]};
1665  pft[0]->m_n[1]=pn[mtch];
1666  pft[1]->m_n[0]=pn[1-mtch];
1667  done=true;
1668  }
1669  }
1670  for(i=0,ni=m_faces.size();i<ni;++i)
1671  {
1672  for(int k=2,l=0;l<3;k=l++)
1673  {
1674  const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
1675  if(mtch!=-1)
1676  {
1677  appendFace(i);
1678  Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
1679  pft[0]->m_n[l]=pn[mtch];
1680  pft[1]->m_n[k]=pn[1-mtch];
1681  appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1682  appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1683  }
1684  }
1685  }
1686  if(!done)
1687  {
1688  m_ndbvt.remove(pn[0]->m_leaf);
1689  m_ndbvt.remove(pn[1]->m_leaf);
1690  m_nodes.pop_back();
1691  m_nodes.pop_back();
1692  }
1693  return(done);
1694 }
1695 
1696 //
1697 bool btSoftBody::rayTest(const btVector3& rayFrom,
1698  const btVector3& rayTo,
1699  sRayCast& results)
1700 {
1701  if(m_faces.size()&&m_fdbvt.empty())
1702  initializeFaceTree();
1703 
1704  results.body = this;
1705  results.fraction = 1.f;
1706  results.feature = eFeature::None;
1707  results.index = -1;
1708 
1709  return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
1710 }
1711 
1712 //
1713 void btSoftBody::setSolver(eSolverPresets::_ preset)
1714 {
1715  m_cfg.m_vsequence.clear();
1716  m_cfg.m_psequence.clear();
1717  m_cfg.m_dsequence.clear();
1718  switch(preset)
1719  {
1720  case eSolverPresets::Positions:
1721  m_cfg.m_psequence.push_back(ePSolver::Anchors);
1722  m_cfg.m_psequence.push_back(ePSolver::RContacts);
1723  m_cfg.m_psequence.push_back(ePSolver::SContacts);
1724  m_cfg.m_psequence.push_back(ePSolver::Linear);
1725  break;
1726  case eSolverPresets::Velocities:
1727  m_cfg.m_vsequence.push_back(eVSolver::Linear);
1728 
1729  m_cfg.m_psequence.push_back(ePSolver::Anchors);
1730  m_cfg.m_psequence.push_back(ePSolver::RContacts);
1731  m_cfg.m_psequence.push_back(ePSolver::SContacts);
1732 
1733  m_cfg.m_dsequence.push_back(ePSolver::Linear);
1734  break;
1735  }
1736 }
1737 
1738 //
1739 void btSoftBody::predictMotion(btScalar dt)
1740 {
1741 
1742  int i,ni;
1743 
1744  /* Update */
1745  if(m_bUpdateRtCst)
1746  {
1747  m_bUpdateRtCst=false;
1748  updateConstants();
1749  m_fdbvt.clear();
1750  if(m_cfg.collisions&fCollision::VF_SS)
1751  {
1752  initializeFaceTree();
1753  }
1754  }
1755 
1756  /* Prepare */
1757  m_sst.sdt = dt*m_cfg.timescale;
1758  m_sst.isdt = 1/m_sst.sdt;
1759  m_sst.velmrg = m_sst.sdt*3;
1760  m_sst.radmrg = getCollisionShape()->getMargin();
1761  m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
1762  /* Forces */
1763  addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
1764  applyForces();
1765  /* Integrate */
1766  for(i=0,ni=m_nodes.size();i<ni;++i)
1767  {
1768  Node& n=m_nodes[i];
1769  n.m_q = n.m_x;
1770  btVector3 deltaV = n.m_f*n.m_im*m_sst.sdt;
1771  {
1772  btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
1773  btScalar clampDeltaV = maxDisplacement/m_sst.sdt;
1774  for (int c=0;c<3;c++)
1775  {
1776  if (deltaV[c]>clampDeltaV)
1777  {
1778  deltaV[c] = clampDeltaV;
1779  }
1780  if (deltaV[c]<-clampDeltaV)
1781  {
1782  deltaV[c]=-clampDeltaV;
1783  }
1784  }
1785  }
1786  n.m_v += deltaV;
1787  n.m_x += n.m_v*m_sst.sdt;
1788  n.m_f = btVector3(0,0,0);
1789  }
1790  /* Clusters */
1791  updateClusters();
1792  /* Bounds */
1793  updateBounds();
1794  /* Nodes */
1795  ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
1796  for(i=0,ni=m_nodes.size();i<ni;++i)
1797  {
1798  Node& n=m_nodes[i];
1799  vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
1800  m_ndbvt.update( n.m_leaf,
1801  vol,
1802  n.m_v*m_sst.velmrg,
1803  m_sst.updmrg);
1804  }
1805  /* Faces */
1806  if(!m_fdbvt.empty())
1807  {
1808  for(int i=0;i<m_faces.size();++i)
1809  {
1810  Face& f=m_faces[i];
1811  const btVector3 v=( f.m_n[0]->m_v+
1812  f.m_n[1]->m_v+
1813  f.m_n[2]->m_v)/3;
1814  vol = VolumeOf(f,m_sst.radmrg);
1815  m_fdbvt.update( f.m_leaf,
1816  vol,
1817  v*m_sst.velmrg,
1818  m_sst.updmrg);
1819  }
1820  }
1821  /* Pose */
1822  updatePose();
1823  /* Match */
1824  if(m_pose.m_bframe&&(m_cfg.kMT>0))
1825  {
1826  const btMatrix3x3 posetrs=m_pose.m_rot;
1827  for(int i=0,ni=m_nodes.size();i<ni;++i)
1828  {
1829  Node& n=m_nodes[i];
1830  if(n.m_im>0)
1831  {
1832  const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1833  n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1834  }
1835  }
1836  }
1837  /* Clear contacts */
1838  m_rcontacts.resize(0);
1839  m_scontacts.resize(0);
1840  /* Optimize dbvt's */
1841  m_ndbvt.optimizeIncremental(1);
1842  m_fdbvt.optimizeIncremental(1);
1843  m_cdbvt.optimizeIncremental(1);
1844 }
1845 
1846 //
1847 void btSoftBody::solveConstraints()
1848 {
1849 
1850  /* Apply clusters */
1851  applyClusters(false);
1852  /* Prepare links */
1853 
1854  int i,ni;
1855 
1856  for(i=0,ni=m_links.size();i<ni;++i)
1857  {
1858  Link& l=m_links[i];
1859  l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q;
1860  l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
1861  }
1862  /* Prepare anchors */
1863  for(i=0,ni=m_anchors.size();i<ni;++i)
1864  {
1865  Anchor& a=m_anchors[i];
1866  const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1867  a.m_c0 = ImpulseMatrix( m_sst.sdt,
1868  a.m_node->m_im,
1869  a.m_body->getInvMass(),
1870  a.m_body->getInvInertiaTensorWorld(),
1871  ra);
1872  a.m_c1 = ra;
1873  a.m_c2 = m_sst.sdt*a.m_node->m_im;
1874  a.m_body->activate();
1875  }
1876  /* Solve velocities */
1877  if(m_cfg.viterations>0)
1878  {
1879  /* Solve */
1880  for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1881  {
1882  for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1883  {
1884  getSolver(m_cfg.m_vsequence[iseq])(this,1);
1885  }
1886  }
1887  /* Update */
1888  for(i=0,ni=m_nodes.size();i<ni;++i)
1889  {
1890  Node& n=m_nodes[i];
1891  n.m_x = n.m_q+n.m_v*m_sst.sdt;
1892  }
1893  }
1894  /* Solve positions */
1895  if(m_cfg.piterations>0)
1896  {
1897  for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1898  {
1899  const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1900  for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1901  {
1902  getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1903  }
1904  }
1905  const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
1906  for(i=0,ni=m_nodes.size();i<ni;++i)
1907  {
1908  Node& n=m_nodes[i];
1909  n.m_v = (n.m_x-n.m_q)*vc;
1910  n.m_f = btVector3(0,0,0);
1911  }
1912  }
1913  /* Solve drift */
1914  if(m_cfg.diterations>0)
1915  {
1916  const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
1917  for(i=0,ni=m_nodes.size();i<ni;++i)
1918  {
1919  Node& n=m_nodes[i];
1920  n.m_q = n.m_x;
1921  }
1922  for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1923  {
1924  for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1925  {
1926  getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1927  }
1928  }
1929  for(int i=0,ni=m_nodes.size();i<ni;++i)
1930  {
1931  Node& n=m_nodes[i];
1932  n.m_v += (n.m_x-n.m_q)*vcf;
1933  }
1934  }
1935  /* Apply clusters */
1936  dampClusters();
1937  applyClusters(true);
1938 }
1939 
1940 //
1941 void btSoftBody::staticSolve(int iterations)
1942 {
1943  for(int isolve=0;isolve<iterations;++isolve)
1944  {
1945  for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1946  {
1947  getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1948  }
1949  }
1950 }
1951 
1952 //
1953 void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
1954 {
1956 }
1957 
1958 //
1959 void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1960 {
1961  const int nb=bodies.size();
1962  int iterations=0;
1963  int i;
1964 
1965  for(i=0;i<nb;++i)
1966  {
1967  iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1968  }
1969  for(i=0;i<nb;++i)
1970  {
1971  bodies[i]->prepareClusters(iterations);
1972  }
1973  for(i=0;i<iterations;++i)
1974  {
1975  const btScalar sor=1;
1976  for(int j=0;j<nb;++j)
1977  {
1978  bodies[j]->solveClusters(sor);
1979  }
1980  }
1981  for(i=0;i<nb;++i)
1982  {
1983  bodies[i]->cleanupClusters();
1984  }
1985 }
1986 
1987 //
1988 void btSoftBody::integrateMotion()
1989 {
1990  /* Update */
1991  updateNormals();
1992 }
1993 
1994 //
1995 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
1996 {
1997  m_rayFrom = rayFrom;
1998  m_rayNormalizedDirection = (rayTo-rayFrom);
1999  m_rayTo = rayTo;
2000  m_mint = mxt;
2001  m_face = 0;
2002  m_tests = 0;
2003 }
2004 
2005 //
2006 void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
2007 {
2008  btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
2009  const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection,
2010  f.m_n[0]->m_x,
2011  f.m_n[1]->m_x,
2012  f.m_n[2]->m_x,
2013  m_mint);
2014  if((t>0)&&(t<m_mint))
2015  {
2016  m_mint=t;m_face=&f;
2017  }
2018  ++m_tests;
2019 }
2020 
2021 //
2022 btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
2023  const btVector3& rayTo,
2024  const btVector3& rayNormalizedDirection,
2025  const btVector3& a,
2026  const btVector3& b,
2027  const btVector3& c,
2028  btScalar maxt)
2029 {
2030  static const btScalar ceps=-SIMD_EPSILON*10;
2031  static const btScalar teps=SIMD_EPSILON*10;
2032 
2033  const btVector3 n=btCross(b-a,c-a);
2034  const btScalar d=btDot(a,n);
2035  const btScalar den=btDot(rayNormalizedDirection,n);
2036  if(!btFuzzyZero(den))
2037  {
2038  const btScalar num=btDot(rayFrom,n)-d;
2039  const btScalar t=-num/den;
2040  if((t>teps)&&(t<maxt))
2041  {
2042  const btVector3 hit=rayFrom+rayNormalizedDirection*t;
2043  if( (btDot(n,btCross(a-hit,b-hit))>ceps) &&
2044  (btDot(n,btCross(b-hit,c-hit))>ceps) &&
2045  (btDot(n,btCross(c-hit,a-hit))>ceps))
2046  {
2047  return(t);
2048  }
2049  }
2050  }
2051  return(-1);
2052 }
2053 
2054 //
2055 void btSoftBody::pointersToIndices()
2056 {
2057 #define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
2058  btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0] : 0;
2059  int i,ni;
2060 
2061  for(i=0,ni=m_nodes.size();i<ni;++i)
2062  {
2063  if(m_nodes[i].m_leaf)
2064  {
2065  m_nodes[i].m_leaf->data=*(void**)&i;
2066  }
2067  }
2068  for(i=0,ni=m_links.size();i<ni;++i)
2069  {
2070  m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
2071  m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
2072  }
2073  for(i=0,ni=m_faces.size();i<ni;++i)
2074  {
2075  m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
2076  m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
2077  m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
2078  if(m_faces[i].m_leaf)
2079  {
2080  m_faces[i].m_leaf->data=*(void**)&i;
2081  }
2082  }
2083  for(i=0,ni=m_anchors.size();i<ni;++i)
2084  {
2085  m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
2086  }
2087  for(i=0,ni=m_notes.size();i<ni;++i)
2088  {
2089  for(int j=0;j<m_notes[i].m_rank;++j)
2090  {
2091  m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
2092  }
2093  }
2094 #undef PTR2IDX
2095 }
2096 
2097 //
2098 void btSoftBody::indicesToPointers(const int* map)
2099 {
2100 #define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
2101  (&(_b_)[(((char*)_p_)-(char*)0)])
2102  btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0]:0;
2103  int i,ni;
2104 
2105  for(i=0,ni=m_nodes.size();i<ni;++i)
2106  {
2107  if(m_nodes[i].m_leaf)
2108  {
2109  m_nodes[i].m_leaf->data=&m_nodes[i];
2110  }
2111  }
2112  for(i=0,ni=m_links.size();i<ni;++i)
2113  {
2114  m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
2115  m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
2116  }
2117  for(i=0,ni=m_faces.size();i<ni;++i)
2118  {
2119  m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
2120  m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
2121  m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
2122  if(m_faces[i].m_leaf)
2123  {
2124  m_faces[i].m_leaf->data=&m_faces[i];
2125  }
2126  }
2127  for(i=0,ni=m_anchors.size();i<ni;++i)
2128  {
2129  m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
2130  }
2131  for(i=0,ni=m_notes.size();i<ni;++i)
2132  {
2133  for(int j=0;j<m_notes[i].m_rank;++j)
2134  {
2135  m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
2136  }
2137  }
2138 #undef IDX2PTR
2139 }
2140 
2141 //
2142 int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
2143  btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
2144 {
2145  int cnt=0;
2146  btVector3 dir = rayTo-rayFrom;
2147 
2148 
2149  if(bcountonly||m_fdbvt.empty())
2150  {/* Full search */
2151 
2152  for(int i=0,ni=m_faces.size();i<ni;++i)
2153  {
2154  const btSoftBody::Face& f=m_faces[i];
2155 
2156  const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir,
2157  f.m_n[0]->m_x,
2158  f.m_n[1]->m_x,
2159  f.m_n[2]->m_x,
2160  mint);
2161  if(t>0)
2162  {
2163  ++cnt;
2164  if(!bcountonly)
2165  {
2166  feature=btSoftBody::eFeature::Face;
2167  index=i;
2168  mint=t;
2169  }
2170  }
2171  }
2172  }
2173  else
2174  {/* Use dbvt */
2175  RayFromToCaster collider(rayFrom,rayTo,mint);
2176 
2177  btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
2178  if(collider.m_face)
2179  {
2180  mint=collider.m_mint;
2181  feature=btSoftBody::eFeature::Face;
2182  index=(int)(collider.m_face-&m_faces[0]);
2183  cnt=1;
2184  }
2185  }
2186 
2187  for (int i=0;i<m_tetras.size();i++)
2188  {
2189  const btSoftBody::Tetra& tet = m_tetras[i];
2190  int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}};
2191  for (int f=0;f<4;f++)
2192  {
2193 
2194  int index0=tetfaces[f][0];
2195  int index1=tetfaces[f][1];
2196  int index2=tetfaces[f][2];
2197  btVector3 v0=tet.m_n[index0]->m_x;
2198  btVector3 v1=tet.m_n[index1]->m_x;
2199  btVector3 v2=tet.m_n[index2]->m_x;
2200 
2201 
2202  const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir,
2203  v0,v1,v2,
2204  mint);
2205  if(t>0)
2206  {
2207  ++cnt;
2208  if(!bcountonly)
2209  {
2210  feature=btSoftBody::eFeature::Tetra;
2211  index=i;
2212  mint=t;
2213  }
2214  }
2215  }
2216  }
2217  return(cnt);
2218 }
2219 
2220 //
2221 void btSoftBody::initializeFaceTree()
2222 {
2223  m_fdbvt.clear();
2224  for(int i=0;i<m_faces.size();++i)
2225  {
2226  Face& f=m_faces[i];
2227  f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
2228  }
2229 }
2230 
2231 //
2232 btVector3 btSoftBody::evaluateCom() const
2233 {
2234  btVector3 com(0,0,0);
2235  if(m_pose.m_bframe)
2236  {
2237  for(int i=0,ni=m_nodes.size();i<ni;++i)
2238  {
2239  com+=m_nodes[i].m_x*m_pose.m_wgh[i];
2240  }
2241  }
2242  return(com);
2243 }
2244 
2245 //
2246 bool btSoftBody::checkContact( const btCollisionObjectWrapper* colObjWrap,
2247  const btVector3& x,
2248  btScalar margin,
2249  btSoftBody::sCti& cti) const
2250 {
2251  btVector3 nrm;
2252  const btCollisionShape *shp = colObjWrap->getCollisionShape();
2253 // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2254  //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2255  const btTransform &wtr = colObjWrap->getWorldTransform();
2256  //todo: check which transform is needed here
2257 
2258  btScalar dst =
2259  m_worldInfo->m_sparsesdf.Evaluate(
2260  wtr.invXform(x),
2261  shp,
2262  nrm,
2263  margin);
2264  if(dst<0)
2265  {
2266  cti.m_colObj = colObjWrap->getCollisionObject();
2267  cti.m_normal = wtr.getBasis()*nrm;
2268  cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
2269  return(true);
2270  }
2271  return(false);
2272 }
2273 
2274 //
2275 void btSoftBody::updateNormals()
2276 {
2277 
2278  const btVector3 zv(0,0,0);
2279  int i,ni;
2280 
2281  for(i=0,ni=m_nodes.size();i<ni;++i)
2282  {
2283  m_nodes[i].m_n=zv;
2284  }
2285  for(i=0,ni=m_faces.size();i<ni;++i)
2286  {
2287  btSoftBody::Face& f=m_faces[i];
2288  const btVector3 n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
2289  f.m_n[2]->m_x-f.m_n[0]->m_x);
2290  f.m_normal=n.normalized();
2291  f.m_n[0]->m_n+=n;
2292  f.m_n[1]->m_n+=n;
2293  f.m_n[2]->m_n+=n;
2294  }
2295  for(i=0,ni=m_nodes.size();i<ni;++i)
2296  {
2297  btScalar len = m_nodes[i].m_n.length();
2298  if (len>SIMD_EPSILON)
2299  m_nodes[i].m_n /= len;
2300  }
2301 }
2302 
2303 //
2304 void btSoftBody::updateBounds()
2305 {
2306  /*if( m_acceleratedSoftBody )
2307  {
2308  // If we have an accelerated softbody we need to obtain the bounds correctly
2309  // For now (slightly hackily) just have a very large AABB
2310  // TODO: Write get bounds kernel
2311  // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2312  // probably do a test and exchange reasonably efficiently.
2313 
2314  m_bounds[0] = btVector3(-1000, -1000, -1000);
2315  m_bounds[1] = btVector3(1000, 1000, 1000);
2316 
2317  } else {*/
2318  if(m_ndbvt.m_root)
2319  {
2320  const btVector3& mins=m_ndbvt.m_root->volume.Mins();
2321  const btVector3& maxs=m_ndbvt.m_root->volume.Maxs();
2322  const btScalar csm=getCollisionShape()->getMargin();
2323  const btVector3 mrg=btVector3( csm,
2324  csm,
2325  csm)*1; // ??? to investigate...
2326  m_bounds[0]=mins-mrg;
2327  m_bounds[1]=maxs+mrg;
2328  if(0!=getBroadphaseHandle())
2329  {
2330  m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
2331  m_bounds[0],
2332  m_bounds[1],
2333  m_worldInfo->m_dispatcher);
2334  }
2335  }
2336  else
2337  {
2338  m_bounds[0]=
2339  m_bounds[1]=btVector3(0,0,0);
2340  }
2341  //}
2342 }
2343 
2344 
2345 //
2346 void btSoftBody::updatePose()
2347 {
2348  if(m_pose.m_bframe)
2349  {
2350  btSoftBody::Pose& pose=m_pose;
2351  const btVector3 com=evaluateCom();
2352  /* Com */
2353  pose.m_com = com;
2354  /* Rotation */
2355  btMatrix3x3 Apq;
2356  const btScalar eps=SIMD_EPSILON;
2357  Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
2358  Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
2359  for(int i=0,ni=m_nodes.size();i<ni;++i)
2360  {
2361  const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
2362  const btVector3& b=pose.m_pos[i];
2363  Apq[0]+=a.x()*b;
2364  Apq[1]+=a.y()*b;
2365  Apq[2]+=a.z()*b;
2366  }
2367  btMatrix3x3 r,s;
2368  PolarDecompose(Apq,r,s);
2369  pose.m_rot=r;
2370  pose.m_scl=pose.m_aqq*r.transpose()*Apq;
2371  if(m_cfg.maxvolume>1)
2372  {
2373  const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
2374  1,m_cfg.maxvolume);
2375  pose.m_scl=Mul(pose.m_scl,idet);
2376  }
2377 
2378  }
2379 }
2380 
2381 //
2382 void btSoftBody::updateArea(bool averageArea)
2383 {
2384  int i,ni;
2385 
2386  /* Face area */
2387  for(i=0,ni=m_faces.size();i<ni;++i)
2388  {
2389  Face& f=m_faces[i];
2390  f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
2391  }
2392 
2393  /* Node area */
2394 
2395  if (averageArea)
2396  {
2397  btAlignedObjectArray<int> counts;
2398  counts.resize(m_nodes.size(),0);
2399  for(i=0,ni=m_nodes.size();i<ni;++i)
2400  {
2401  m_nodes[i].m_area = 0;
2402  }
2403  for(i=0,ni=m_faces.size();i<ni;++i)
2404  {
2405  btSoftBody::Face& f=m_faces[i];
2406  for(int j=0;j<3;++j)
2407  {
2408  const int index=(int)(f.m_n[j]-&m_nodes[0]);
2409  counts[index]++;
2410  f.m_n[j]->m_area+=btFabs(f.m_ra);
2411  }
2412  }
2413  for(i=0,ni=m_nodes.size();i<ni;++i)
2414  {
2415  if(counts[i]>0)
2416  m_nodes[i].m_area/=(btScalar)counts[i];
2417  else
2418  m_nodes[i].m_area=0;
2419  }
2420  }
2421  else
2422  {
2423  // initialize node area as zero
2424  for(i=0,ni=m_nodes.size();i<ni;++i)
2425  {
2426  m_nodes[i].m_area=0;
2427  }
2428 
2429  for(i=0,ni=m_faces.size();i<ni;++i)
2430  {
2431  btSoftBody::Face& f=m_faces[i];
2432 
2433  for(int j=0;j<3;++j)
2434  {
2435  f.m_n[j]->m_area += f.m_ra;
2436  }
2437  }
2438 
2439  for(i=0,ni=m_nodes.size();i<ni;++i)
2440  {
2441  m_nodes[i].m_area *= 0.3333333f;
2442  }
2443  }
2444 }
2445 
2446 
2447 void btSoftBody::updateLinkConstants()
2448 {
2449  int i,ni;
2450 
2451  /* Links */
2452  for(i=0,ni=m_links.size();i<ni;++i)
2453  {
2454  Link& l=m_links[i];
2455  Material& m=*l.m_material;
2456  l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
2457  }
2458 }
2459 
2460 void btSoftBody::updateConstants()
2461 {
2462  resetLinkRestLengths();
2463  updateLinkConstants();
2464  updateArea();
2465 }
2466 
2467 
2468 
2469 //
2470 void btSoftBody::initializeClusters()
2471 {
2472  int i;
2473 
2474  for( i=0;i<m_clusters.size();++i)
2475  {
2476  Cluster& c=*m_clusters[i];
2477  c.m_imass=0;
2478  c.m_masses.resize(c.m_nodes.size());
2479  for(int j=0;j<c.m_nodes.size();++j)
2480  {
2481  if (c.m_nodes[j]->m_im==0)
2482  {
2483  c.m_containsAnchor = true;
2484  c.m_masses[j] = BT_LARGE_FLOAT;
2485  } else
2486  {
2487  c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im;
2488  }
2489  c.m_imass += c.m_masses[j];
2490  }
2491  c.m_imass = btScalar(1.)/c.m_imass;
2492  c.m_com = btSoftBody::clusterCom(&c);
2493  c.m_lv = btVector3(0,0,0);
2494  c.m_av = btVector3(0,0,0);
2495  c.m_leaf = 0;
2496  /* Inertia */
2497  btMatrix3x3& ii=c.m_locii;
2498  ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
2499  {
2500  int i,ni;
2501 
2502  for(i=0,ni=c.m_nodes.size();i<ni;++i)
2503  {
2504  const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2505  const btVector3 q=k*k;
2506  const btScalar m=c.m_masses[i];
2507  ii[0][0] += m*(q[1]+q[2]);
2508  ii[1][1] += m*(q[0]+q[2]);
2509  ii[2][2] += m*(q[0]+q[1]);
2510  ii[0][1] -= m*k[0]*k[1];
2511  ii[0][2] -= m*k[0]*k[2];
2512  ii[1][2] -= m*k[1]*k[2];
2513  }
2514  }
2515  ii[1][0]=ii[0][1];
2516  ii[2][0]=ii[0][2];
2517  ii[2][1]=ii[1][2];
2518 
2519  ii = ii.inverse();
2520 
2521  /* Frame */
2522  c.m_framexform.setIdentity();
2523  c.m_framexform.setOrigin(c.m_com);
2524  c.m_framerefs.resize(c.m_nodes.size());
2525  {
2526  int i;
2527  for(i=0;i<c.m_framerefs.size();++i)
2528  {
2529  c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
2530  }
2531  }
2532  }
2533 }
2534 
2535 //
2536 void btSoftBody::updateClusters()
2537 {
2538  BT_PROFILE("UpdateClusters");
2539  int i;
2540 
2541  for(i=0;i<m_clusters.size();++i)
2542  {
2543  btSoftBody::Cluster& c=*m_clusters[i];
2544  const int n=c.m_nodes.size();
2545  //const btScalar invn=1/(btScalar)n;
2546  if(n)
2547  {
2548  /* Frame */
2549  const btScalar eps=btScalar(0.0001);
2550  btMatrix3x3 m,r,s;
2551  m[0]=m[1]=m[2]=btVector3(0,0,0);
2552  m[0][0]=eps*1;
2553  m[1][1]=eps*2;
2554  m[2][2]=eps*3;
2555  c.m_com=clusterCom(&c);
2556  for(int i=0;i<c.m_nodes.size();++i)
2557  {
2558  const btVector3 a=c.m_nodes[i]->m_x-c.m_com;
2559  const btVector3& b=c.m_framerefs[i];
2560  m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
2561  }
2562  PolarDecompose(m,r,s);
2563  c.m_framexform.setOrigin(c.m_com);
2564  c.m_framexform.setBasis(r);
2565  /* Inertia */
2566 #if 1/* Constant */
2567  c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
2568 #else
2569 #if 0/* Sphere */
2570  const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
2571  const btVector3 inertia(rk,rk,rk);
2572  const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2573  btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2574  btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2575 
2576  c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2577 #else/* Actual */
2578  c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
2579  for(int i=0;i<n;++i)
2580  {
2581  const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2582  const btVector3 q=k*k;
2583  const btScalar m=1/c.m_nodes[i]->m_im;
2584  c.m_invwi[0][0] += m*(q[1]+q[2]);
2585  c.m_invwi[1][1] += m*(q[0]+q[2]);
2586  c.m_invwi[2][2] += m*(q[0]+q[1]);
2587  c.m_invwi[0][1] -= m*k[0]*k[1];
2588  c.m_invwi[0][2] -= m*k[0]*k[2];
2589  c.m_invwi[1][2] -= m*k[1]*k[2];
2590  }
2591  c.m_invwi[1][0]=c.m_invwi[0][1];
2592  c.m_invwi[2][0]=c.m_invwi[0][2];
2593  c.m_invwi[2][1]=c.m_invwi[1][2];
2594  c.m_invwi=c.m_invwi.inverse();
2595 #endif
2596 #endif
2597  /* Velocities */
2598  c.m_lv=btVector3(0,0,0);
2599  c.m_av=btVector3(0,0,0);
2600  {
2601  int i;
2602 
2603  for(i=0;i<n;++i)
2604  {
2605  const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
2606  c.m_lv += v;
2607  c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v);
2608  }
2609  }
2610  c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
2611  c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
2612  c.m_vimpulses[0] =
2613  c.m_vimpulses[1] = btVector3(0,0,0);
2614  c.m_dimpulses[0] =
2615  c.m_dimpulses[1] = btVector3(0,0,0);
2616  c.m_nvimpulses = 0;
2617  c.m_ndimpulses = 0;
2618  /* Matching */
2619  if(c.m_matching>0)
2620  {
2621  for(int j=0;j<c.m_nodes.size();++j)
2622  {
2623  Node& n=*c.m_nodes[j];
2624  const btVector3 x=c.m_framexform*c.m_framerefs[j];
2625  n.m_x=Lerp(n.m_x,x,c.m_matching);
2626  }
2627  }
2628  /* Dbvt */
2629  if(c.m_collide)
2630  {
2631  btVector3 mi=c.m_nodes[0]->m_x;
2632  btVector3 mx=mi;
2633  for(int j=1;j<n;++j)
2634  {
2635  mi.setMin(c.m_nodes[j]->m_x);
2636  mx.setMax(c.m_nodes[j]->m_x);
2637  }
2638  ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
2639  if(c.m_leaf)
2640  m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
2641  else
2642  c.m_leaf=m_cdbvt.insert(bounds,&c);
2643  }
2644  }
2645  }
2646 
2647 
2648 }
2649 
2650 
2651 
2652 
2653 //
2654 void btSoftBody::cleanupClusters()
2655 {
2656  for(int i=0;i<m_joints.size();++i)
2657  {
2658  m_joints[i]->Terminate(m_sst.sdt);
2659  if(m_joints[i]->m_delete)
2660  {
2661  btAlignedFree(m_joints[i]);
2662  m_joints.remove(m_joints[i--]);
2663  }
2664  }
2665 }
2666 
2667 //
2668 void btSoftBody::prepareClusters(int iterations)
2669 {
2670  for(int i=0;i<m_joints.size();++i)
2671  {
2672  m_joints[i]->Prepare(m_sst.sdt,iterations);
2673  }
2674 }
2675 
2676 
2677 //
2678 void btSoftBody::solveClusters(btScalar sor)
2679 {
2680  for(int i=0,ni=m_joints.size();i<ni;++i)
2681  {
2682  m_joints[i]->Solve(m_sst.sdt,sor);
2683  }
2684 }
2685 
2686 //
2687 void btSoftBody::applyClusters(bool drift)
2688 {
2689  BT_PROFILE("ApplyClusters");
2690 // const btScalar f0=m_sst.sdt;
2691  //const btScalar f1=f0/2;
2692  btAlignedObjectArray<btVector3> deltas;
2693  btAlignedObjectArray<btScalar> weights;
2694  deltas.resize(m_nodes.size(),btVector3(0,0,0));
2695  weights.resize(m_nodes.size(),0);
2696  int i;
2697 
2698  if(drift)
2699  {
2700  for(i=0;i<m_clusters.size();++i)
2701  {
2702  Cluster& c=*m_clusters[i];
2703  if(c.m_ndimpulses)
2704  {
2705  c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2706  c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2707  }
2708  }
2709  }
2710 
2711  for(i=0;i<m_clusters.size();++i)
2712  {
2713  Cluster& c=*m_clusters[i];
2714  if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2715  {
2716  const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2717  const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2718  for(int j=0;j<c.m_nodes.size();++j)
2719  {
2720  const int idx=int(c.m_nodes[j]-&m_nodes[0]);
2721  const btVector3& x=c.m_nodes[j]->m_x;
2722  const btScalar q=c.m_masses[j];
2723  deltas[idx] += (v+btCross(w,x-c.m_com))*q;
2724  weights[idx] += q;
2725  }
2726  }
2727  }
2728  for(i=0;i<deltas.size();++i)
2729  {
2730  if(weights[i]>0)
2731  {
2732  m_nodes[i].m_x+=deltas[i]/weights[i];
2733  }
2734  }
2735 }
2736 
2737 //
2738 void btSoftBody::dampClusters()
2739 {
2740  int i;
2741 
2742  for(i=0;i<m_clusters.size();++i)
2743  {
2744  Cluster& c=*m_clusters[i];
2745  if(c.m_ndamping>0)
2746  {
2747  for(int j=0;j<c.m_nodes.size();++j)
2748  {
2749  Node& n=*c.m_nodes[j];
2750  if(n.m_im>0)
2751  {
2752  const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2753  if(vx.length2()<=n.m_v.length2())
2754  {
2755  n.m_v += c.m_ndamping*(vx-n.m_v);
2756  }
2757  }
2758  }
2759  }
2760  }
2761 }
2762 
2763 //
2764 void btSoftBody::Joint::Prepare(btScalar dt,int)
2765 {
2766  m_bodies[0].activate();
2767  m_bodies[1].activate();
2768 }
2769 
2770 //
2771 void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2772 {
2773  static const btScalar maxdrift=4;
2774  Joint::Prepare(dt,iterations);
2775  m_rpos[0] = m_bodies[0].xform()*m_refs[0];
2776  m_rpos[1] = m_bodies[1].xform()*m_refs[1];
2777  m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2778  m_rpos[0] -= m_bodies[0].xform().getOrigin();
2779  m_rpos[1] -= m_bodies[1].xform().getOrigin();
2780  m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2781  m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2782  if(m_split>0)
2783  {
2784  m_sdrift = m_massmatrix*(m_drift*m_split);
2785  m_drift *= 1-m_split;
2786  }
2787  m_drift /=(btScalar)iterations;
2788 }
2789 
2790 //
2791 void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2792 {
2793  const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2794  const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2795  const btVector3 vr=va-vb;
2796  btSoftBody::Impulse impulse;
2797  impulse.m_asVelocity = 1;
2798  impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor;
2799  m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2800  m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2801 }
2802 
2803 //
2804 void btSoftBody::LJoint::Terminate(btScalar dt)
2805 {
2806  if(m_split>0)
2807  {
2808  m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2809  m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2810  }
2811 }
2812 
2813 //
2814 void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2815 {
2816  static const btScalar maxdrift=SIMD_PI/16;
2817  m_icontrol->Prepare(this);
2818  Joint::Prepare(dt,iterations);
2819  m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0];
2820  m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1];
2821  m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0]));
2822  m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
2823  m_drift *= m_erp/dt;
2824  m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2825  if(m_split>0)
2826  {
2827  m_sdrift = m_massmatrix*(m_drift*m_split);
2828  m_drift *= 1-m_split;
2829  }
2830  m_drift /=(btScalar)iterations;
2831 }
2832 
2833 //
2834 void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2835 {
2836  const btVector3 va=m_bodies[0].angularVelocity();
2837  const btVector3 vb=m_bodies[1].angularVelocity();
2838  const btVector3 vr=va-vb;
2839  const btScalar sp=btDot(vr,m_axis[0]);
2840  const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2841  btSoftBody::Impulse impulse;
2842  impulse.m_asVelocity = 1;
2843  impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor;
2844  m_bodies[0].applyAImpulse(-impulse);
2845  m_bodies[1].applyAImpulse( impulse);
2846 }
2847 
2848 //
2849 void btSoftBody::AJoint::Terminate(btScalar dt)
2850 {
2851  if(m_split>0)
2852  {
2853  m_bodies[0].applyDAImpulse(-m_sdrift);
2854  m_bodies[1].applyDAImpulse( m_sdrift);
2855  }
2856 }
2857 
2858 //
2859 void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2860 {
2861  Joint::Prepare(dt,iterations);
2862  const bool dodrift=(m_life==0);
2863  m_delete=(++m_life)>m_maxlife;
2864  if(dodrift)
2865  {
2866  m_drift=m_drift*m_erp/dt;
2867  if(m_split>0)
2868  {
2869  m_sdrift = m_massmatrix*(m_drift*m_split);
2870  m_drift *= 1-m_split;
2871  }
2872  m_drift/=(btScalar)iterations;
2873  }
2874  else
2875  {
2876  m_drift=m_sdrift=btVector3(0,0,0);
2877  }
2878 }
2879 
2880 //
2881 void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2882 {
2883  const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2884  const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2885  const btVector3 vrel=va-vb;
2886  const btScalar rvac=btDot(vrel,m_normal);
2887  btSoftBody::Impulse impulse;
2888  impulse.m_asVelocity = 1;
2889  impulse.m_velocity = m_drift;
2890  if(rvac<0)
2891  {
2892  const btVector3 iv=m_normal*rvac;
2893  const btVector3 fv=vrel-iv;
2894  impulse.m_velocity += iv+fv*m_friction;
2895  }
2896  impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
2897 
2898  if (m_bodies[0].m_soft==m_bodies[1].m_soft)
2899  {
2900  if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
2901  (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
2902  {
2903  if (impulse.m_asVelocity)
2904  {
2905  if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
2906  {
2907 
2908  } else
2909  {
2910  m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
2911  m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
2912  }
2913  }
2914  }
2915  } else
2916  {
2917  m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2918  m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2919  }
2920 }
2921 
2922 //
2923 void btSoftBody::CJoint::Terminate(btScalar dt)
2924 {
2925  if(m_split>0)
2926  {
2927  m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2928  m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2929  }
2930 }
2931 
2932 //
2933 void btSoftBody::applyForces()
2934 {
2935 
2936  BT_PROFILE("SoftBody applyForces");
2937 // const btScalar dt = m_sst.sdt;
2938  const btScalar kLF = m_cfg.kLF;
2939  const btScalar kDG = m_cfg.kDG;
2940  const btScalar kPR = m_cfg.kPR;
2941  const btScalar kVC = m_cfg.kVC;
2942  const bool as_lift = kLF>0;
2943  const bool as_drag = kDG>0;
2944  const bool as_pressure = kPR!=0;
2945  const bool as_volume = kVC>0;
2946  const bool as_aero = as_lift ||
2947  as_drag ;
2948  //const bool as_vaero = as_aero &&
2949  // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
2950  //const bool as_faero = as_aero &&
2951  // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
2952  const bool use_medium = as_aero;
2953  const bool use_volume = as_pressure ||
2954  as_volume ;
2955  btScalar volume = 0;
2956  btScalar ivolumetp = 0;
2957  btScalar dvolumetv = 0;
2958  btSoftBody::sMedium medium;
2959  if(use_volume)
2960  {
2961  volume = getVolume();
2962  ivolumetp = 1/btFabs(volume)*kPR;
2963  dvolumetv = (m_pose.m_volume-volume)*kVC;
2964  }
2965  /* Per vertex forces */
2966  int i,ni;
2967 
2968  for(i=0,ni=m_nodes.size();i<ni;++i)
2969  {
2970  btSoftBody::Node& n=m_nodes[i];
2971  if(n.m_im>0)
2972  {
2973  if(use_medium)
2974  {
2975  /* Aerodynamics */
2976  addAeroForceToNode(m_windVelocity, i);
2977  }
2978  /* Pressure */
2979  if(as_pressure)
2980  {
2981  n.m_f += n.m_n*(n.m_area*ivolumetp);
2982  }
2983  /* Volume */
2984  if(as_volume)
2985  {
2986  n.m_f += n.m_n*(n.m_area*dvolumetv);
2987  }
2988  }
2989  }
2990 
2991  /* Per face forces */
2992  for(i=0,ni=m_faces.size();i<ni;++i)
2993  {
2994  // btSoftBody::Face& f=m_faces[i];
2995 
2996  /* Aerodynamics */
2997  addAeroForceToFace(m_windVelocity, i);
2998  }
2999 }
3000 
3001 //
3002 void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
3003 {
3004  const btScalar kAHR=psb->m_cfg.kAHR*kst;
3005  const btScalar dt=psb->m_sst.sdt;
3006  for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
3007  {
3008  const Anchor& a=psb->m_anchors[i];
3009  const btTransform& t=a.m_body->getWorldTransform();
3010  Node& n=*a.m_node;
3011  const btVector3 wa=t*a.m_local;
3012  const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
3013  const btVector3 vb=n.m_x-n.m_q;
3014  const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
3015  const btVector3 impulse=a.m_c0*vr*a.m_influence;
3016  n.m_x+=impulse*a.m_c2;
3017  a.m_body->applyImpulse(-impulse,a.m_c1);
3018  }
3019 }
3020 
3021 //
3022 void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
3023 {
3024  const btScalar dt = psb->m_sst.sdt;
3025  const btScalar mrg = psb->getCollisionShape()->getMargin();
3026  for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
3027  {
3028  const RContact& c = psb->m_rcontacts[i];
3029  const sCti& cti = c.m_cti;
3030  btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3031 
3032  const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
3033  const btVector3 vb = c.m_node->m_x-c.m_node->m_q;
3034  const btVector3 vr = vb-va;
3035  const btScalar dn = btDot(vr, cti.m_normal);
3036  if(dn<=SIMD_EPSILON)
3037  {
3038  const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
3039  const btVector3 fv = vr - (cti.m_normal * dn);
3040  // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3041  const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
3042  c.m_node->m_x -= impulse * c.m_c2;
3043  if (tmpRigid)
3044  tmpRigid->applyImpulse(impulse,c.m_c1);
3045  }
3046  }
3047 }
3048 
3049 //
3050 void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
3051 {
3052  for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
3053  {
3054  const SContact& c=psb->m_scontacts[i];
3055  const btVector3& nr=c.m_normal;
3056  Node& n=*c.m_node;
3057  Face& f=*c.m_face;
3058  const btVector3 p=BaryEval( f.m_n[0]->m_x,
3059  f.m_n[1]->m_x,
3060  f.m_n[2]->m_x,
3061  c.m_weights);
3062  const btVector3 q=BaryEval( f.m_n[0]->m_q,
3063  f.m_n[1]->m_q,
3064  f.m_n[2]->m_q,
3065  c.m_weights);
3066  const btVector3 vr=(n.m_x-n.m_q)-(p-q);
3067  btVector3 corr(0,0,0);
3068  btScalar dot = btDot(vr,nr);
3069  if(dot<0)
3070  {
3071  const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
3072  corr+=c.m_normal*j;
3073  }
3074  corr -= ProjectOnPlane(vr,nr)*c.m_friction;
3075  n.m_x += corr*c.m_cfm[0];
3076  f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x());
3077  f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y());
3078  f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z());
3079  }
3080 }
3081 
3082 //
3083 void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
3084 {
3085  for(int i=0,ni=psb->m_links.size();i<ni;++i)
3086  {
3087  Link& l=psb->m_links[i];
3088  if(l.m_c0>0)
3089  {
3090  Node& a=*l.m_n[0];
3091  Node& b=*l.m_n[1];
3092  const btVector3 del=b.m_x-a.m_x;
3093  const btScalar len=del.length2();
3094  if (l.m_c1+len > SIMD_EPSILON)
3095  {
3096  const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
3097  a.m_x-=del*(k*a.m_im);
3098  b.m_x+=del*(k*b.m_im);
3099  }
3100  }
3101  }
3102 }
3103 
3104 //
3105 void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
3106 {
3107  for(int i=0,ni=psb->m_links.size();i<ni;++i)
3108  {
3109  Link& l=psb->m_links[i];
3110  Node** n=l.m_n;
3111  const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
3112  n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
3113  n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
3114  }
3115 }
3116 
3117 //
3118 btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
3119 {
3120  switch(solver)
3121  {
3122  case ePSolver::Anchors:
3123  return(&btSoftBody::PSolve_Anchors);
3124  case ePSolver::Linear:
3125  return(&btSoftBody::PSolve_Links);
3126  case ePSolver::RContacts:
3127  return(&btSoftBody::PSolve_RContacts);
3128  case ePSolver::SContacts:
3129  return(&btSoftBody::PSolve_SContacts);
3130  default:
3131  {
3132  }
3133  }
3134  return(0);
3135 }
3136 
3137 //
3138 btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
3139 {
3140  switch(solver)
3141  {
3142  case eVSolver::Linear: return(&btSoftBody::VSolve_Links);
3143  default:
3144  {
3145  }
3146  }
3147  return(0);
3148 }
3149 
3150 //
3151 void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
3152 {
3153 
3154  switch(m_cfg.collisions&fCollision::RVSmask)
3155  {
3156  case fCollision::SDF_RS:
3157  {
3158  btSoftColliders::CollideSDF_RS docollide;
3159  btRigidBody* prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject());
3160  btTransform wtr=pcoWrap->getWorldTransform();
3161 
3162  const btTransform ctr=pcoWrap->getWorldTransform();
3163  const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
3164  const btScalar basemargin=getCollisionShape()->getMargin();
3165  btVector3 mins;
3166  btVector3 maxs;
3167  ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
3168  pcoWrap->getCollisionShape()->getAabb( pcoWrap->getWorldTransform(),
3169  mins,
3170  maxs);
3171  volume=btDbvtVolume::FromMM(mins,maxs);
3172  volume.Expand(btVector3(basemargin,basemargin,basemargin));
3173  docollide.psb = this;
3174  docollide.m_colObj1Wrap = pcoWrap;
3175  docollide.m_rigidBody = prb1;
3176 
3177  docollide.dynmargin = basemargin+timemargin;
3178  docollide.stamargin = basemargin;
3179  m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
3180  }
3181  break;
3182  case fCollision::CL_RS:
3183  {
3184  btSoftColliders::CollideCL_RS collider;
3185  collider.ProcessColObj(this,pcoWrap);
3186  }
3187  break;
3188  }
3189 }
3190 
3191 //
3192 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
3193 {
3194  const int cf=m_cfg.collisions&psb->m_cfg.collisions;
3195  switch(cf&fCollision::SVSmask)
3196  {
3197  case fCollision::CL_SS:
3198  {
3199 
3200  //support self-collision if CL_SELF flag set
3201  if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
3202  {
3203  btSoftColliders::CollideCL_SS docollide;
3204  docollide.ProcessSoftSoft(this,psb);
3205  }
3206 
3207  }
3208  break;
3209  case fCollision::VF_SS:
3210  {
3211  //only self-collision for Cluster, not Vertex-Face yet
3212  if (this!=psb)
3213  {
3214  btSoftColliders::CollideVF_SS docollide;
3215  /* common */
3216  docollide.mrg= getCollisionShape()->getMargin()+
3217  psb->getCollisionShape()->getMargin();
3218  /* psb0 nodes vs psb1 faces */
3219  docollide.psb[0]=this;
3220  docollide.psb[1]=psb;
3221  docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
3222  docollide.psb[1]->m_fdbvt.m_root,
3223  docollide);
3224  /* psb1 nodes vs psb0 faces */
3225  docollide.psb[0]=psb;
3226  docollide.psb[1]=this;
3227  docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
3228  docollide.psb[1]->m_fdbvt.m_root,
3229  docollide);
3230  }
3231  }
3232  break;
3233  default:
3234  {
3235 
3236  }
3237  }
3238 }
3239 
3240 
3241 
3242 void btSoftBody::setWindVelocity( const btVector3 &velocity )
3243 {
3244  m_windVelocity = velocity;
3245 }
3246 
3247 
3248 const btVector3& btSoftBody::getWindVelocity()
3249 {
3250  return m_windVelocity;
3251 }
3252 
3253 
3254 
3255 int btSoftBody::calculateSerializeBufferSize() const
3256 {
3257  int sz = sizeof(btSoftBodyData);
3258  return sz;
3259 }
3260 
3262 const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
3263 {
3264  btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer;
3265 
3266  btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
3267 
3268  btHashMap<btHashPtr,int> m_nodeIndexMap;
3269 
3270  sbd->m_numMaterials = m_materials.size();
3271  sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0;
3272 
3273  if (sbd->m_materials)
3274  {
3275  int sz = sizeof(SoftBodyMaterialData*);
3276  int numElem = sbd->m_numMaterials;
3277  btChunk* chunk = serializer->allocate(sz,numElem);
3278  //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
3279  SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
3280  for (int i=0;i<numElem;i++,memPtr++)
3281  {
3282  btSoftBody::Material* mat = m_materials[i];
3283  *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
3284  if (!serializer->findPointer(mat))
3285  {
3286  //serialize it here
3287  btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1);
3288  SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
3289  memPtr->m_flags = mat->m_flags;
3290  memPtr->m_angularStiffness = mat->m_kAST;
3291  memPtr->m_linearStiffness = mat->m_kLST;
3292  memPtr->m_volumeStiffness = mat->m_kVST;
3293  serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat);
3294  }
3295  }
3296  serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials);
3297  }
3298 
3299 
3300 
3301 
3302  sbd->m_numNodes = m_nodes.size();
3303  sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0;
3304  if (sbd->m_nodes)
3305  {
3306  int sz = sizeof(SoftBodyNodeData);
3307  int numElem = sbd->m_numNodes;
3308  btChunk* chunk = serializer->allocate(sz,numElem);
3309  SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
3310  for (int i=0;i<numElem;i++,memPtr++)
3311  {
3312  m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce);
3313  memPtr->m_area = m_nodes[i].m_area;
3314  memPtr->m_attach = m_nodes[i].m_battach;
3315  memPtr->m_inverseMass = m_nodes[i].m_im;
3316  memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0;
3317  m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
3318  m_nodes[i].m_x.serializeFloat(memPtr->m_position);
3319  m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
3320  m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
3321  m_nodeIndexMap.insert(&m_nodes[i],i);
3322  }
3323  serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes);
3324  }
3325 
3326  sbd->m_numLinks = m_links.size();
3327  sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
3328  if (sbd->m_links)
3329  {
3330  int sz = sizeof(SoftBodyLinkData);
3331  int numElem = sbd->m_numLinks;
3332  btChunk* chunk = serializer->allocate(sz,numElem);
3333  SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
3334  for (int i=0;i<numElem;i++,memPtr++)
3335  {
3336  memPtr->m_bbending = m_links[i].m_bbending;
3337  memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0;
3338  memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1;
3339  memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1;
3340  btAssert(memPtr->m_nodeIndices[0]<m_nodes.size());
3341  btAssert(memPtr->m_nodeIndices[1]<m_nodes.size());
3342  memPtr->m_restLength = m_links[i].m_rl;
3343  }
3344  serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]);
3345 
3346  }
3347 
3348 
3349  sbd->m_numFaces = m_faces.size();
3350  sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0;
3351  if (sbd->m_faces)
3352  {
3353  int sz = sizeof(SoftBodyFaceData);
3354  int numElem = sbd->m_numFaces;
3355  btChunk* chunk = serializer->allocate(sz,numElem);
3356  SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
3357  for (int i=0;i<numElem;i++,memPtr++)
3358  {
3359  memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0;
3360  m_faces[i].m_normal.serializeFloat( memPtr->m_normal);
3361  for (int j=0;j<3;j++)
3362  {
3363  memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1;
3364  }
3365  memPtr->m_restArea = m_faces[i].m_ra;
3366  }
3367  serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]);
3368  }
3369 
3370 
3371  sbd->m_numTetrahedra = m_tetras.size();
3372  sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0;
3373  if (sbd->m_tetrahedra)
3374  {
3375  int sz = sizeof(SoftBodyTetraData);
3376  int numElem = sbd->m_numTetrahedra;
3377  btChunk* chunk = serializer->allocate(sz,numElem);
3378  SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
3379  for (int i=0;i<numElem;i++,memPtr++)
3380  {
3381  for (int j=0;j<4;j++)
3382  {
3383  m_tetras[i].m_c0[j].serializeFloat( memPtr->m_c0[j] );
3384  memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1;
3385  }
3386  memPtr->m_c1 = m_tetras[i].m_c1;
3387  memPtr->m_c2 = m_tetras[i].m_c2;
3388  memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0;
3389  memPtr->m_restVolume = m_tetras[i].m_rv;
3390  }
3391  serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]);
3392  }
3393 
3394  sbd->m_numAnchors = m_anchors.size();
3395  sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0;
3396  if (sbd->m_anchors)
3397  {
3398  int sz = sizeof(SoftRigidAnchorData);
3399  int numElem = sbd->m_numAnchors;
3400  btChunk* chunk = serializer->allocate(sz,numElem);
3401  SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
3402  for (int i=0;i<numElem;i++,memPtr++)
3403  {
3404  m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
3405  m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
3406  memPtr->m_c2 = m_anchors[i].m_c2;
3407  m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
3408  memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1;
3409 
3410  memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*) serializer->getUniquePointer((void*)m_anchors[i].m_body): 0;
3411  btAssert(memPtr->m_nodeIndex < m_nodes.size());
3412  }
3413  serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]);
3414  }
3415 
3416 
3417  sbd->m_config.m_dynamicFriction = m_cfg.kDF;
3418  sbd->m_config.m_baumgarte = m_cfg.kVCF;
3419  sbd->m_config.m_pressure = m_cfg.kPR;
3420  sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
3421  sbd->m_config.m_lift = m_cfg.kLF;
3422  sbd->m_config.m_drag = m_cfg.kDG;
3423  sbd->m_config.m_positionIterations = m_cfg.piterations;
3424  sbd->m_config.m_driftIterations = m_cfg.diterations;
3425  sbd->m_config.m_clusterIterations = m_cfg.citerations;
3426  sbd->m_config.m_velocityIterations = m_cfg.viterations;
3427  sbd->m_config.m_maxVolume = m_cfg.maxvolume;
3428  sbd->m_config.m_damping = m_cfg.kDP;
3429  sbd->m_config.m_poseMatch = m_cfg.kMT;
3430  sbd->m_config.m_collisionFlags = m_cfg.collisions;
3431  sbd->m_config.m_volume = m_cfg.kVC;
3432  sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
3433  sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
3434  sbd->m_config.m_softContactHardness = m_cfg.kSHR;
3435  sbd->m_config.m_anchorHardness = m_cfg.kAHR;
3436  sbd->m_config.m_timeScale = m_cfg.timescale;
3437  sbd->m_config.m_maxVolume = m_cfg.maxvolume;
3438  sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
3439  sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
3440  sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
3441  sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
3442  sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
3443  sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
3444 
3445  //pose for shape matching
3446  {
3447  sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
3448 
3449  int sz = sizeof(SoftBodyPoseData);
3450  btChunk* chunk = serializer->allocate(sz,1);
3451  SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
3452 
3453  m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
3454  memPtr->m_bframe = m_pose.m_bframe;
3455  memPtr->m_bvolume = m_pose.m_bvolume;
3456  m_pose.m_com.serializeFloat(memPtr->m_com);
3457 
3458  memPtr->m_numPositions = m_pose.m_pos.size();
3459  memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0;
3460  if (memPtr->m_numPositions)
3461  {
3462  int numElem = memPtr->m_numPositions;
3463  int sz = sizeof(btVector3Data);
3464  btChunk* chunk = serializer->allocate(sz,numElem);
3465  btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
3466  for (int i=0;i<numElem;i++,memPtr++)
3467  {
3468  m_pose.m_pos[i].serializeFloat(*memPtr);
3469  }
3470  serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]);
3471  }
3472  memPtr->m_restVolume = m_pose.m_volume;
3473  m_pose.m_rot.serializeFloat(memPtr->m_rot);
3474  m_pose.m_scl.serializeFloat(memPtr->m_scale);
3475 
3476  memPtr->m_numWeigts = m_pose.m_wgh.size();
3477  memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0;
3478  if (memPtr->m_numWeigts)
3479  {
3480 
3481  int numElem = memPtr->m_numWeigts;
3482  int sz = sizeof(float);
3483  btChunk* chunk = serializer->allocate(sz,numElem);
3484  float* memPtr = (float*) chunk->m_oldPtr;
3485  for (int i=0;i<numElem;i++,memPtr++)
3486  {
3487  *memPtr = m_pose.m_wgh[i];
3488  }
3489  serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]);
3490  }
3491 
3492  serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose);
3493  }
3494 
3495  //clusters for convex-cluster collision detection
3496 
3497  sbd->m_numClusters = m_clusters.size();
3498  sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0;
3499  if (sbd->m_numClusters)
3500  {
3501  int numElem = sbd->m_numClusters;
3502  int sz = sizeof(SoftBodyClusterData);
3503  btChunk* chunk = serializer->allocate(sz,numElem);
3504  SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
3505  for (int i=0;i<numElem;i++,memPtr++)
3506  {
3507  memPtr->m_adamping= m_clusters[i]->m_adamping;
3508  m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
3509  memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
3510  memPtr->m_collide = m_clusters[i]->m_collide;
3511  m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
3512  memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
3513  m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
3514  m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
3515  m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
3516  memPtr->m_idmass = m_clusters[i]->m_idmass;
3517  memPtr->m_imass = m_clusters[i]->m_imass;
3518  m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
3519  memPtr->m_ldamping = m_clusters[i]->m_ldamping;
3520  m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
3521  m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
3522  memPtr->m_matching = m_clusters[i]->m_matching;
3523  memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
3524  memPtr->m_ndamping = m_clusters[i]->m_ndamping;
3525  memPtr->m_ldamping = m_clusters[i]->m_ldamping;
3526  memPtr->m_adamping = m_clusters[i]->m_adamping;
3527  memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
3528 
3529  memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
3530  memPtr->m_numMasses = m_clusters[i]->m_masses.size();
3531  memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
3532 
3533  memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
3534  m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
3535  m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
3536  memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
3537 
3538 
3539 
3540  memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
3541  if (memPtr->m_framerefs)
3542  {
3543  int numElem = memPtr->m_numFrameRefs;
3544  int sz = sizeof(btVector3FloatData);
3545  btChunk* chunk = serializer->allocate(sz,numElem);
3546  btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
3547  for (int j=0;j<numElem;j++,memPtr++)
3548  {
3549  m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
3550  }
3551  serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]);
3552  }
3553 
3554  memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0;
3555  if (memPtr->m_masses)
3556  {
3557  int numElem = memPtr->m_numMasses;
3558  int sz = sizeof(float);
3559  btChunk* chunk = serializer->allocate(sz,numElem);
3560  float* memPtr = (float*) chunk->m_oldPtr;
3561  for (int j=0;j<numElem;j++,memPtr++)
3562  {
3563  *memPtr = m_clusters[i]->m_masses[j];
3564  }
3565  serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]);
3566  }
3567 
3568  memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0;
3569  if (memPtr->m_nodeIndices )
3570  {
3571  int numElem = memPtr->m_numMasses;
3572  int sz = sizeof(int);
3573  btChunk* chunk = serializer->allocate(sz,numElem);
3574  int* memPtr = (int*) chunk->m_oldPtr;
3575  for (int j=0;j<numElem;j++,memPtr++)
3576  {
3577  int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
3578  btAssert(indexPtr);
3579  *memPtr = *indexPtr;
3580  }
3581  serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes);
3582  }
3583  }
3584  serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]);
3585 
3586  }
3587 
3588 
3589 
3590  sbd->m_numJoints = m_joints.size();
3591  sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0;
3592 
3593  if (sbd->m_joints)
3594  {
3595  int sz = sizeof(btSoftBodyJointData);
3596  int numElem = m_joints.size();
3597  btChunk* chunk = serializer->allocate(sz,numElem);
3598  btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
3599 
3600  for (int i=0;i<numElem;i++,memPtr++)
3601  {
3602  memPtr->m_jointType = (int)m_joints[i]->Type();
3603  m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
3604  m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
3605  memPtr->m_cfm = m_joints[i]->m_cfm;
3606  memPtr->m_erp = m_joints[i]->m_erp;
3607  memPtr->m_split = m_joints[i]->m_split;
3608  memPtr->m_delete = m_joints[i]->m_delete;
3609 
3610  for (int j=0;j<4;j++)
3611  {
3612  memPtr->m_relPosition[0].m_floats[j] = 0.f;
3613  memPtr->m_relPosition[1].m_floats[j] = 0.f;
3614  }
3615  memPtr->m_bodyA = 0;
3616  memPtr->m_bodyB = 0;
3617  if (m_joints[i]->m_bodies[0].m_soft)
3618  {
3619  memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
3620  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
3621  }
3622  if (m_joints[i]->m_bodies[0].m_collisionObject)
3623  {
3624  memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
3625  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
3626  }
3627  if (m_joints[i]->m_bodies[0].m_rigid)
3628  {
3629  memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
3630  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
3631  }
3632 
3633  if (m_joints[i]->m_bodies[1].m_soft)
3634  {
3635  memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
3636  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
3637  }
3638  if (m_joints[i]->m_bodies[1].m_collisionObject)
3639  {
3640  memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
3641  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
3642  }
3643  if (m_joints[i]->m_bodies[1].m_rigid)
3644  {
3645  memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
3646  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
3647  }
3648  }
3649  serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]);
3650  }
3651 
3652 
3653  return btSoftBodyDataName;
3654 }
3655 
Generated on Wed Apr 9 2014 02:38:24 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2