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  n.m_v += n.m_f*n.m_im*m_sst.sdt;
1771  n.m_x += n.m_v*m_sst.sdt;
1772  n.m_f = btVector3(0,0,0);
1773  }
1774  /* Clusters */
1775  updateClusters();
1776  /* Bounds */
1777  updateBounds();
1778  /* Nodes */
1779  ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
1780  for(i=0,ni=m_nodes.size();i<ni;++i)
1781  {
1782  Node& n=m_nodes[i];
1783  vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
1784  m_ndbvt.update( n.m_leaf,
1785  vol,
1786  n.m_v*m_sst.velmrg,
1787  m_sst.updmrg);
1788  }
1789  /* Faces */
1790  if(!m_fdbvt.empty())
1791  {
1792  for(int i=0;i<m_faces.size();++i)
1793  {
1794  Face& f=m_faces[i];
1795  const btVector3 v=( f.m_n[0]->m_v+
1796  f.m_n[1]->m_v+
1797  f.m_n[2]->m_v)/3;
1798  vol = VolumeOf(f,m_sst.radmrg);
1799  m_fdbvt.update( f.m_leaf,
1800  vol,
1801  v*m_sst.velmrg,
1802  m_sst.updmrg);
1803  }
1804  }
1805  /* Pose */
1806  updatePose();
1807  /* Match */
1808  if(m_pose.m_bframe&&(m_cfg.kMT>0))
1809  {
1810  const btMatrix3x3 posetrs=m_pose.m_rot;
1811  for(int i=0,ni=m_nodes.size();i<ni;++i)
1812  {
1813  Node& n=m_nodes[i];
1814  if(n.m_im>0)
1815  {
1816  const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1817  n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1818  }
1819  }
1820  }
1821  /* Clear contacts */
1822  m_rcontacts.resize(0);
1823  m_scontacts.resize(0);
1824  /* Optimize dbvt's */
1825  m_ndbvt.optimizeIncremental(1);
1826  m_fdbvt.optimizeIncremental(1);
1827  m_cdbvt.optimizeIncremental(1);
1828 }
1829 
1830 //
1831 void btSoftBody::solveConstraints()
1832 {
1833 
1834  /* Apply clusters */
1835  applyClusters(false);
1836  /* Prepare links */
1837 
1838  int i,ni;
1839 
1840  for(i=0,ni=m_links.size();i<ni;++i)
1841  {
1842  Link& l=m_links[i];
1843  l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q;
1844  l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
1845  }
1846  /* Prepare anchors */
1847  for(i=0,ni=m_anchors.size();i<ni;++i)
1848  {
1849  Anchor& a=m_anchors[i];
1850  const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1851  a.m_c0 = ImpulseMatrix( m_sst.sdt,
1852  a.m_node->m_im,
1853  a.m_body->getInvMass(),
1854  a.m_body->getInvInertiaTensorWorld(),
1855  ra);
1856  a.m_c1 = ra;
1857  a.m_c2 = m_sst.sdt*a.m_node->m_im;
1858  a.m_body->activate();
1859  }
1860  /* Solve velocities */
1861  if(m_cfg.viterations>0)
1862  {
1863  /* Solve */
1864  for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1865  {
1866  for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1867  {
1868  getSolver(m_cfg.m_vsequence[iseq])(this,1);
1869  }
1870  }
1871  /* Update */
1872  for(i=0,ni=m_nodes.size();i<ni;++i)
1873  {
1874  Node& n=m_nodes[i];
1875  n.m_x = n.m_q+n.m_v*m_sst.sdt;
1876  }
1877  }
1878  /* Solve positions */
1879  if(m_cfg.piterations>0)
1880  {
1881  for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1882  {
1883  const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1884  for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1885  {
1886  getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1887  }
1888  }
1889  const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
1890  for(i=0,ni=m_nodes.size();i<ni;++i)
1891  {
1892  Node& n=m_nodes[i];
1893  n.m_v = (n.m_x-n.m_q)*vc;
1894  n.m_f = btVector3(0,0,0);
1895  }
1896  }
1897  /* Solve drift */
1898  if(m_cfg.diterations>0)
1899  {
1900  const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
1901  for(i=0,ni=m_nodes.size();i<ni;++i)
1902  {
1903  Node& n=m_nodes[i];
1904  n.m_q = n.m_x;
1905  }
1906  for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1907  {
1908  for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1909  {
1910  getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1911  }
1912  }
1913  for(int i=0,ni=m_nodes.size();i<ni;++i)
1914  {
1915  Node& n=m_nodes[i];
1916  n.m_v += (n.m_x-n.m_q)*vcf;
1917  }
1918  }
1919  /* Apply clusters */
1920  dampClusters();
1921  applyClusters(true);
1922 }
1923 
1924 //
1925 void btSoftBody::staticSolve(int iterations)
1926 {
1927  for(int isolve=0;isolve<iterations;++isolve)
1928  {
1929  for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1930  {
1931  getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1932  }
1933  }
1934 }
1935 
1936 //
1937 void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
1938 {
1940 }
1941 
1942 //
1943 void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1944 {
1945  const int nb=bodies.size();
1946  int iterations=0;
1947  int i;
1948 
1949  for(i=0;i<nb;++i)
1950  {
1951  iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1952  }
1953  for(i=0;i<nb;++i)
1954  {
1955  bodies[i]->prepareClusters(iterations);
1956  }
1957  for(i=0;i<iterations;++i)
1958  {
1959  const btScalar sor=1;
1960  for(int j=0;j<nb;++j)
1961  {
1962  bodies[j]->solveClusters(sor);
1963  }
1964  }
1965  for(i=0;i<nb;++i)
1966  {
1967  bodies[i]->cleanupClusters();
1968  }
1969 }
1970 
1971 //
1972 void btSoftBody::integrateMotion()
1973 {
1974  /* Update */
1975  updateNormals();
1976 }
1977 
1978 //
1979 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
1980 {
1981  m_rayFrom = rayFrom;
1982  m_rayNormalizedDirection = (rayTo-rayFrom);
1983  m_rayTo = rayTo;
1984  m_mint = mxt;
1985  m_face = 0;
1986  m_tests = 0;
1987 }
1988 
1989 //
1990 void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
1991 {
1992  btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
1993  const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection,
1994  f.m_n[0]->m_x,
1995  f.m_n[1]->m_x,
1996  f.m_n[2]->m_x,
1997  m_mint);
1998  if((t>0)&&(t<m_mint))
1999  {
2000  m_mint=t;m_face=&f;
2001  }
2002  ++m_tests;
2003 }
2004 
2005 //
2006 btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
2007  const btVector3& rayTo,
2008  const btVector3& rayNormalizedDirection,
2009  const btVector3& a,
2010  const btVector3& b,
2011  const btVector3& c,
2012  btScalar maxt)
2013 {
2014  static const btScalar ceps=-SIMD_EPSILON*10;
2015  static const btScalar teps=SIMD_EPSILON*10;
2016 
2017  const btVector3 n=btCross(b-a,c-a);
2018  const btScalar d=btDot(a,n);
2019  const btScalar den=btDot(rayNormalizedDirection,n);
2020  if(!btFuzzyZero(den))
2021  {
2022  const btScalar num=btDot(rayFrom,n)-d;
2023  const btScalar t=-num/den;
2024  if((t>teps)&&(t<maxt))
2025  {
2026  const btVector3 hit=rayFrom+rayNormalizedDirection*t;
2027  if( (btDot(n,btCross(a-hit,b-hit))>ceps) &&
2028  (btDot(n,btCross(b-hit,c-hit))>ceps) &&
2029  (btDot(n,btCross(c-hit,a-hit))>ceps))
2030  {
2031  return(t);
2032  }
2033  }
2034  }
2035  return(-1);
2036 }
2037 
2038 //
2039 void btSoftBody::pointersToIndices()
2040 {
2041 #define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
2042  btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0] : 0;
2043  int i,ni;
2044 
2045  for(i=0,ni=m_nodes.size();i<ni;++i)
2046  {
2047  if(m_nodes[i].m_leaf)
2048  {
2049  m_nodes[i].m_leaf->data=*(void**)&i;
2050  }
2051  }
2052  for(i=0,ni=m_links.size();i<ni;++i)
2053  {
2054  m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
2055  m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
2056  }
2057  for(i=0,ni=m_faces.size();i<ni;++i)
2058  {
2059  m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
2060  m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
2061  m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
2062  if(m_faces[i].m_leaf)
2063  {
2064  m_faces[i].m_leaf->data=*(void**)&i;
2065  }
2066  }
2067  for(i=0,ni=m_anchors.size();i<ni;++i)
2068  {
2069  m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
2070  }
2071  for(i=0,ni=m_notes.size();i<ni;++i)
2072  {
2073  for(int j=0;j<m_notes[i].m_rank;++j)
2074  {
2075  m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
2076  }
2077  }
2078 #undef PTR2IDX
2079 }
2080 
2081 //
2082 void btSoftBody::indicesToPointers(const int* map)
2083 {
2084 #define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
2085  (&(_b_)[(((char*)_p_)-(char*)0)])
2086  btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0]:0;
2087  int i,ni;
2088 
2089  for(i=0,ni=m_nodes.size();i<ni;++i)
2090  {
2091  if(m_nodes[i].m_leaf)
2092  {
2093  m_nodes[i].m_leaf->data=&m_nodes[i];
2094  }
2095  }
2096  for(i=0,ni=m_links.size();i<ni;++i)
2097  {
2098  m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
2099  m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
2100  }
2101  for(i=0,ni=m_faces.size();i<ni;++i)
2102  {
2103  m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
2104  m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
2105  m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
2106  if(m_faces[i].m_leaf)
2107  {
2108  m_faces[i].m_leaf->data=&m_faces[i];
2109  }
2110  }
2111  for(i=0,ni=m_anchors.size();i<ni;++i)
2112  {
2113  m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
2114  }
2115  for(i=0,ni=m_notes.size();i<ni;++i)
2116  {
2117  for(int j=0;j<m_notes[i].m_rank;++j)
2118  {
2119  m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
2120  }
2121  }
2122 #undef IDX2PTR
2123 }
2124 
2125 //
2126 int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
2127  btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
2128 {
2129  int cnt=0;
2130  btVector3 dir = rayTo-rayFrom;
2131 
2132 
2133  if(bcountonly||m_fdbvt.empty())
2134  {/* Full search */
2135 
2136  for(int i=0,ni=m_faces.size();i<ni;++i)
2137  {
2138  const btSoftBody::Face& f=m_faces[i];
2139 
2140  const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir,
2141  f.m_n[0]->m_x,
2142  f.m_n[1]->m_x,
2143  f.m_n[2]->m_x,
2144  mint);
2145  if(t>0)
2146  {
2147  ++cnt;
2148  if(!bcountonly)
2149  {
2150  feature=btSoftBody::eFeature::Face;
2151  index=i;
2152  mint=t;
2153  }
2154  }
2155  }
2156  }
2157  else
2158  {/* Use dbvt */
2159  RayFromToCaster collider(rayFrom,rayTo,mint);
2160 
2161  btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
2162  if(collider.m_face)
2163  {
2164  mint=collider.m_mint;
2165  feature=btSoftBody::eFeature::Face;
2166  index=(int)(collider.m_face-&m_faces[0]);
2167  cnt=1;
2168  }
2169  }
2170 
2171  for (int i=0;i<m_tetras.size();i++)
2172  {
2173  const btSoftBody::Tetra& tet = m_tetras[i];
2174  int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}};
2175  for (int f=0;f<4;f++)
2176  {
2177 
2178  int index0=tetfaces[f][0];
2179  int index1=tetfaces[f][1];
2180  int index2=tetfaces[f][2];
2181  btVector3 v0=tet.m_n[index0]->m_x;
2182  btVector3 v1=tet.m_n[index1]->m_x;
2183  btVector3 v2=tet.m_n[index2]->m_x;
2184 
2185 
2186  const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir,
2187  v0,v1,v2,
2188  mint);
2189  if(t>0)
2190  {
2191  ++cnt;
2192  if(!bcountonly)
2193  {
2194  feature=btSoftBody::eFeature::Tetra;
2195  index=i;
2196  mint=t;
2197  }
2198  }
2199  }
2200  }
2201  return(cnt);
2202 }
2203 
2204 //
2205 void btSoftBody::initializeFaceTree()
2206 {
2207  m_fdbvt.clear();
2208  for(int i=0;i<m_faces.size();++i)
2209  {
2210  Face& f=m_faces[i];
2211  f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
2212  }
2213 }
2214 
2215 //
2216 btVector3 btSoftBody::evaluateCom() const
2217 {
2218  btVector3 com(0,0,0);
2219  if(m_pose.m_bframe)
2220  {
2221  for(int i=0,ni=m_nodes.size();i<ni;++i)
2222  {
2223  com+=m_nodes[i].m_x*m_pose.m_wgh[i];
2224  }
2225  }
2226  return(com);
2227 }
2228 
2229 //
2230 bool btSoftBody::checkContact( const btCollisionObjectWrapper* colObjWrap,
2231  const btVector3& x,
2232  btScalar margin,
2233  btSoftBody::sCti& cti) const
2234 {
2235  btVector3 nrm;
2236  const btCollisionShape *shp = colObjWrap->getCollisionShape();
2237 // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2238  //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2239  const btTransform &wtr = colObjWrap->getWorldTransform();
2240  //todo: check which transform is needed here
2241 
2242  btScalar dst =
2243  m_worldInfo->m_sparsesdf.Evaluate(
2244  wtr.invXform(x),
2245  shp,
2246  nrm,
2247  margin);
2248  if(dst<0)
2249  {
2250  cti.m_colObj = colObjWrap->getCollisionObject();
2251  cti.m_normal = wtr.getBasis()*nrm;
2252  cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
2253  return(true);
2254  }
2255  return(false);
2256 }
2257 
2258 //
2259 void btSoftBody::updateNormals()
2260 {
2261 
2262  const btVector3 zv(0,0,0);
2263  int i,ni;
2264 
2265  for(i=0,ni=m_nodes.size();i<ni;++i)
2266  {
2267  m_nodes[i].m_n=zv;
2268  }
2269  for(i=0,ni=m_faces.size();i<ni;++i)
2270  {
2271  btSoftBody::Face& f=m_faces[i];
2272  const btVector3 n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
2273  f.m_n[2]->m_x-f.m_n[0]->m_x);
2274  f.m_normal=n.normalized();
2275  f.m_n[0]->m_n+=n;
2276  f.m_n[1]->m_n+=n;
2277  f.m_n[2]->m_n+=n;
2278  }
2279  for(i=0,ni=m_nodes.size();i<ni;++i)
2280  {
2281  btScalar len = m_nodes[i].m_n.length();
2282  if (len>SIMD_EPSILON)
2283  m_nodes[i].m_n /= len;
2284  }
2285 }
2286 
2287 //
2288 void btSoftBody::updateBounds()
2289 {
2290  /*if( m_acceleratedSoftBody )
2291  {
2292  // If we have an accelerated softbody we need to obtain the bounds correctly
2293  // For now (slightly hackily) just have a very large AABB
2294  // TODO: Write get bounds kernel
2295  // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2296  // probably do a test and exchange reasonably efficiently.
2297 
2298  m_bounds[0] = btVector3(-1000, -1000, -1000);
2299  m_bounds[1] = btVector3(1000, 1000, 1000);
2300 
2301  } else {*/
2302  if(m_ndbvt.m_root)
2303  {
2304  const btVector3& mins=m_ndbvt.m_root->volume.Mins();
2305  const btVector3& maxs=m_ndbvt.m_root->volume.Maxs();
2306  const btScalar csm=getCollisionShape()->getMargin();
2307  const btVector3 mrg=btVector3( csm,
2308  csm,
2309  csm)*1; // ??? to investigate...
2310  m_bounds[0]=mins-mrg;
2311  m_bounds[1]=maxs+mrg;
2312  if(0!=getBroadphaseHandle())
2313  {
2314  m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
2315  m_bounds[0],
2316  m_bounds[1],
2317  m_worldInfo->m_dispatcher);
2318  }
2319  }
2320  else
2321  {
2322  m_bounds[0]=
2323  m_bounds[1]=btVector3(0,0,0);
2324  }
2325  //}
2326 }
2327 
2328 
2329 //
2330 void btSoftBody::updatePose()
2331 {
2332  if(m_pose.m_bframe)
2333  {
2334  btSoftBody::Pose& pose=m_pose;
2335  const btVector3 com=evaluateCom();
2336  /* Com */
2337  pose.m_com = com;
2338  /* Rotation */
2339  btMatrix3x3 Apq;
2340  const btScalar eps=SIMD_EPSILON;
2341  Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
2342  Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
2343  for(int i=0,ni=m_nodes.size();i<ni;++i)
2344  {
2345  const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
2346  const btVector3& b=pose.m_pos[i];
2347  Apq[0]+=a.x()*b;
2348  Apq[1]+=a.y()*b;
2349  Apq[2]+=a.z()*b;
2350  }
2351  btMatrix3x3 r,s;
2352  PolarDecompose(Apq,r,s);
2353  pose.m_rot=r;
2354  pose.m_scl=pose.m_aqq*r.transpose()*Apq;
2355  if(m_cfg.maxvolume>1)
2356  {
2357  const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
2358  1,m_cfg.maxvolume);
2359  pose.m_scl=Mul(pose.m_scl,idet);
2360  }
2361 
2362  }
2363 }
2364 
2365 //
2366 void btSoftBody::updateArea(bool averageArea)
2367 {
2368  int i,ni;
2369 
2370  /* Face area */
2371  for(i=0,ni=m_faces.size();i<ni;++i)
2372  {
2373  Face& f=m_faces[i];
2374  f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
2375  }
2376 
2377  /* Node area */
2378 
2379  if (averageArea)
2380  {
2381  btAlignedObjectArray<int> counts;
2382  counts.resize(m_nodes.size(),0);
2383  for(i=0,ni=m_nodes.size();i<ni;++i)
2384  {
2385  m_nodes[i].m_area = 0;
2386  }
2387  for(i=0,ni=m_faces.size();i<ni;++i)
2388  {
2389  btSoftBody::Face& f=m_faces[i];
2390  for(int j=0;j<3;++j)
2391  {
2392  const int index=(int)(f.m_n[j]-&m_nodes[0]);
2393  counts[index]++;
2394  f.m_n[j]->m_area+=btFabs(f.m_ra);
2395  }
2396  }
2397  for(i=0,ni=m_nodes.size();i<ni;++i)
2398  {
2399  if(counts[i]>0)
2400  m_nodes[i].m_area/=(btScalar)counts[i];
2401  else
2402  m_nodes[i].m_area=0;
2403  }
2404  }
2405  else
2406  {
2407  // initialize node area as zero
2408  for(i=0,ni=m_nodes.size();i<ni;++i)
2409  {
2410  m_nodes[i].m_area=0;
2411  }
2412 
2413  for(i=0,ni=m_faces.size();i<ni;++i)
2414  {
2415  btSoftBody::Face& f=m_faces[i];
2416 
2417  for(int j=0;j<3;++j)
2418  {
2419  f.m_n[j]->m_area += f.m_ra;
2420  }
2421  }
2422 
2423  for(i=0,ni=m_nodes.size();i<ni;++i)
2424  {
2425  m_nodes[i].m_area *= 0.3333333f;
2426  }
2427  }
2428 }
2429 
2430 
2431 void btSoftBody::updateLinkConstants()
2432 {
2433  int i,ni;
2434 
2435  /* Links */
2436  for(i=0,ni=m_links.size();i<ni;++i)
2437  {
2438  Link& l=m_links[i];
2439  Material& m=*l.m_material;
2440  l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
2441  }
2442 }
2443 
2444 void btSoftBody::updateConstants()
2445 {
2446  resetLinkRestLengths();
2447  updateLinkConstants();
2448  updateArea();
2449 }
2450 
2451 
2452 
2453 //
2454 void btSoftBody::initializeClusters()
2455 {
2456  int i;
2457 
2458  for( i=0;i<m_clusters.size();++i)
2459  {
2460  Cluster& c=*m_clusters[i];
2461  c.m_imass=0;
2462  c.m_masses.resize(c.m_nodes.size());
2463  for(int j=0;j<c.m_nodes.size();++j)
2464  {
2465  if (c.m_nodes[j]->m_im==0)
2466  {
2467  c.m_containsAnchor = true;
2468  c.m_masses[j] = BT_LARGE_FLOAT;
2469  } else
2470  {
2471  c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im;
2472  }
2473  c.m_imass += c.m_masses[j];
2474  }
2475  c.m_imass = btScalar(1.)/c.m_imass;
2476  c.m_com = btSoftBody::clusterCom(&c);
2477  c.m_lv = btVector3(0,0,0);
2478  c.m_av = btVector3(0,0,0);
2479  c.m_leaf = 0;
2480  /* Inertia */
2481  btMatrix3x3& ii=c.m_locii;
2482  ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
2483  {
2484  int i,ni;
2485 
2486  for(i=0,ni=c.m_nodes.size();i<ni;++i)
2487  {
2488  const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2489  const btVector3 q=k*k;
2490  const btScalar m=c.m_masses[i];
2491  ii[0][0] += m*(q[1]+q[2]);
2492  ii[1][1] += m*(q[0]+q[2]);
2493  ii[2][2] += m*(q[0]+q[1]);
2494  ii[0][1] -= m*k[0]*k[1];
2495  ii[0][2] -= m*k[0]*k[2];
2496  ii[1][2] -= m*k[1]*k[2];
2497  }
2498  }
2499  ii[1][0]=ii[0][1];
2500  ii[2][0]=ii[0][2];
2501  ii[2][1]=ii[1][2];
2502 
2503  ii = ii.inverse();
2504 
2505  /* Frame */
2506  c.m_framexform.setIdentity();
2507  c.m_framexform.setOrigin(c.m_com);
2508  c.m_framerefs.resize(c.m_nodes.size());
2509  {
2510  int i;
2511  for(i=0;i<c.m_framerefs.size();++i)
2512  {
2513  c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
2514  }
2515  }
2516  }
2517 }
2518 
2519 //
2520 void btSoftBody::updateClusters()
2521 {
2522  BT_PROFILE("UpdateClusters");
2523  int i;
2524 
2525  for(i=0;i<m_clusters.size();++i)
2526  {
2527  btSoftBody::Cluster& c=*m_clusters[i];
2528  const int n=c.m_nodes.size();
2529  //const btScalar invn=1/(btScalar)n;
2530  if(n)
2531  {
2532  /* Frame */
2533  const btScalar eps=btScalar(0.0001);
2534  btMatrix3x3 m,r,s;
2535  m[0]=m[1]=m[2]=btVector3(0,0,0);
2536  m[0][0]=eps*1;
2537  m[1][1]=eps*2;
2538  m[2][2]=eps*3;
2539  c.m_com=clusterCom(&c);
2540  for(int i=0;i<c.m_nodes.size();++i)
2541  {
2542  const btVector3 a=c.m_nodes[i]->m_x-c.m_com;
2543  const btVector3& b=c.m_framerefs[i];
2544  m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
2545  }
2546  PolarDecompose(m,r,s);
2547  c.m_framexform.setOrigin(c.m_com);
2548  c.m_framexform.setBasis(r);
2549  /* Inertia */
2550 #if 1/* Constant */
2551  c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
2552 #else
2553 #if 0/* Sphere */
2554  const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
2555  const btVector3 inertia(rk,rk,rk);
2556  const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2557  btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2558  btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2559 
2560  c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2561 #else/* Actual */
2562  c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
2563  for(int i=0;i<n;++i)
2564  {
2565  const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2566  const btVector3 q=k*k;
2567  const btScalar m=1/c.m_nodes[i]->m_im;
2568  c.m_invwi[0][0] += m*(q[1]+q[2]);
2569  c.m_invwi[1][1] += m*(q[0]+q[2]);
2570  c.m_invwi[2][2] += m*(q[0]+q[1]);
2571  c.m_invwi[0][1] -= m*k[0]*k[1];
2572  c.m_invwi[0][2] -= m*k[0]*k[2];
2573  c.m_invwi[1][2] -= m*k[1]*k[2];
2574  }
2575  c.m_invwi[1][0]=c.m_invwi[0][1];
2576  c.m_invwi[2][0]=c.m_invwi[0][2];
2577  c.m_invwi[2][1]=c.m_invwi[1][2];
2578  c.m_invwi=c.m_invwi.inverse();
2579 #endif
2580 #endif
2581  /* Velocities */
2582  c.m_lv=btVector3(0,0,0);
2583  c.m_av=btVector3(0,0,0);
2584  {
2585  int i;
2586 
2587  for(i=0;i<n;++i)
2588  {
2589  const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
2590  c.m_lv += v;
2591  c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v);
2592  }
2593  }
2594  c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
2595  c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
2596  c.m_vimpulses[0] =
2597  c.m_vimpulses[1] = btVector3(0,0,0);
2598  c.m_dimpulses[0] =
2599  c.m_dimpulses[1] = btVector3(0,0,0);
2600  c.m_nvimpulses = 0;
2601  c.m_ndimpulses = 0;
2602  /* Matching */
2603  if(c.m_matching>0)
2604  {
2605  for(int j=0;j<c.m_nodes.size();++j)
2606  {
2607  Node& n=*c.m_nodes[j];
2608  const btVector3 x=c.m_framexform*c.m_framerefs[j];
2609  n.m_x=Lerp(n.m_x,x,c.m_matching);
2610  }
2611  }
2612  /* Dbvt */
2613  if(c.m_collide)
2614  {
2615  btVector3 mi=c.m_nodes[0]->m_x;
2616  btVector3 mx=mi;
2617  for(int j=1;j<n;++j)
2618  {
2619  mi.setMin(c.m_nodes[j]->m_x);
2620  mx.setMax(c.m_nodes[j]->m_x);
2621  }
2622  ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
2623  if(c.m_leaf)
2624  m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
2625  else
2626  c.m_leaf=m_cdbvt.insert(bounds,&c);
2627  }
2628  }
2629  }
2630 
2631 
2632 }
2633 
2634 
2635 
2636 
2637 //
2638 void btSoftBody::cleanupClusters()
2639 {
2640  for(int i=0;i<m_joints.size();++i)
2641  {
2642  m_joints[i]->Terminate(m_sst.sdt);
2643  if(m_joints[i]->m_delete)
2644  {
2645  btAlignedFree(m_joints[i]);
2646  m_joints.remove(m_joints[i--]);
2647  }
2648  }
2649 }
2650 
2651 //
2652 void btSoftBody::prepareClusters(int iterations)
2653 {
2654  for(int i=0;i<m_joints.size();++i)
2655  {
2656  m_joints[i]->Prepare(m_sst.sdt,iterations);
2657  }
2658 }
2659 
2660 
2661 //
2662 void btSoftBody::solveClusters(btScalar sor)
2663 {
2664  for(int i=0,ni=m_joints.size();i<ni;++i)
2665  {
2666  m_joints[i]->Solve(m_sst.sdt,sor);
2667  }
2668 }
2669 
2670 //
2671 void btSoftBody::applyClusters(bool drift)
2672 {
2673  BT_PROFILE("ApplyClusters");
2674 // const btScalar f0=m_sst.sdt;
2675  //const btScalar f1=f0/2;
2676  btAlignedObjectArray<btVector3> deltas;
2677  btAlignedObjectArray<btScalar> weights;
2678  deltas.resize(m_nodes.size(),btVector3(0,0,0));
2679  weights.resize(m_nodes.size(),0);
2680  int i;
2681 
2682  if(drift)
2683  {
2684  for(i=0;i<m_clusters.size();++i)
2685  {
2686  Cluster& c=*m_clusters[i];
2687  if(c.m_ndimpulses)
2688  {
2689  c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2690  c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2691  }
2692  }
2693  }
2694 
2695  for(i=0;i<m_clusters.size();++i)
2696  {
2697  Cluster& c=*m_clusters[i];
2698  if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2699  {
2700  const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2701  const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2702  for(int j=0;j<c.m_nodes.size();++j)
2703  {
2704  const int idx=int(c.m_nodes[j]-&m_nodes[0]);
2705  const btVector3& x=c.m_nodes[j]->m_x;
2706  const btScalar q=c.m_masses[j];
2707  deltas[idx] += (v+btCross(w,x-c.m_com))*q;
2708  weights[idx] += q;
2709  }
2710  }
2711  }
2712  for(i=0;i<deltas.size();++i)
2713  {
2714  if(weights[i]>0)
2715  {
2716  m_nodes[i].m_x+=deltas[i]/weights[i];
2717  }
2718  }
2719 }
2720 
2721 //
2722 void btSoftBody::dampClusters()
2723 {
2724  int i;
2725 
2726  for(i=0;i<m_clusters.size();++i)
2727  {
2728  Cluster& c=*m_clusters[i];
2729  if(c.m_ndamping>0)
2730  {
2731  for(int j=0;j<c.m_nodes.size();++j)
2732  {
2733  Node& n=*c.m_nodes[j];
2734  if(n.m_im>0)
2735  {
2736  const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2737  if(vx.length2()<=n.m_v.length2())
2738  {
2739  n.m_v += c.m_ndamping*(vx-n.m_v);
2740  }
2741  }
2742  }
2743  }
2744  }
2745 }
2746 
2747 //
2748 void btSoftBody::Joint::Prepare(btScalar dt,int)
2749 {
2750  m_bodies[0].activate();
2751  m_bodies[1].activate();
2752 }
2753 
2754 //
2755 void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2756 {
2757  static const btScalar maxdrift=4;
2758  Joint::Prepare(dt,iterations);
2759  m_rpos[0] = m_bodies[0].xform()*m_refs[0];
2760  m_rpos[1] = m_bodies[1].xform()*m_refs[1];
2761  m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2762  m_rpos[0] -= m_bodies[0].xform().getOrigin();
2763  m_rpos[1] -= m_bodies[1].xform().getOrigin();
2764  m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2765  m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2766  if(m_split>0)
2767  {
2768  m_sdrift = m_massmatrix*(m_drift*m_split);
2769  m_drift *= 1-m_split;
2770  }
2771  m_drift /=(btScalar)iterations;
2772 }
2773 
2774 //
2775 void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2776 {
2777  const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2778  const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2779  const btVector3 vr=va-vb;
2780  btSoftBody::Impulse impulse;
2781  impulse.m_asVelocity = 1;
2782  impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor;
2783  m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2784  m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2785 }
2786 
2787 //
2788 void btSoftBody::LJoint::Terminate(btScalar dt)
2789 {
2790  if(m_split>0)
2791  {
2792  m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2793  m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2794  }
2795 }
2796 
2797 //
2798 void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2799 {
2800  static const btScalar maxdrift=SIMD_PI/16;
2801  m_icontrol->Prepare(this);
2802  Joint::Prepare(dt,iterations);
2803  m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0];
2804  m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1];
2805  m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0]));
2806  m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
2807  m_drift *= m_erp/dt;
2808  m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2809  if(m_split>0)
2810  {
2811  m_sdrift = m_massmatrix*(m_drift*m_split);
2812  m_drift *= 1-m_split;
2813  }
2814  m_drift /=(btScalar)iterations;
2815 }
2816 
2817 //
2818 void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2819 {
2820  const btVector3 va=m_bodies[0].angularVelocity();
2821  const btVector3 vb=m_bodies[1].angularVelocity();
2822  const btVector3 vr=va-vb;
2823  const btScalar sp=btDot(vr,m_axis[0]);
2824  const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2825  btSoftBody::Impulse impulse;
2826  impulse.m_asVelocity = 1;
2827  impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor;
2828  m_bodies[0].applyAImpulse(-impulse);
2829  m_bodies[1].applyAImpulse( impulse);
2830 }
2831 
2832 //
2833 void btSoftBody::AJoint::Terminate(btScalar dt)
2834 {
2835  if(m_split>0)
2836  {
2837  m_bodies[0].applyDAImpulse(-m_sdrift);
2838  m_bodies[1].applyDAImpulse( m_sdrift);
2839  }
2840 }
2841 
2842 //
2843 void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2844 {
2845  Joint::Prepare(dt,iterations);
2846  const bool dodrift=(m_life==0);
2847  m_delete=(++m_life)>m_maxlife;
2848  if(dodrift)
2849  {
2850  m_drift=m_drift*m_erp/dt;
2851  if(m_split>0)
2852  {
2853  m_sdrift = m_massmatrix*(m_drift*m_split);
2854  m_drift *= 1-m_split;
2855  }
2856  m_drift/=(btScalar)iterations;
2857  }
2858  else
2859  {
2860  m_drift=m_sdrift=btVector3(0,0,0);
2861  }
2862 }
2863 
2864 //
2865 void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2866 {
2867  const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2868  const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2869  const btVector3 vrel=va-vb;
2870  const btScalar rvac=btDot(vrel,m_normal);
2871  btSoftBody::Impulse impulse;
2872  impulse.m_asVelocity = 1;
2873  impulse.m_velocity = m_drift;
2874  if(rvac<0)
2875  {
2876  const btVector3 iv=m_normal*rvac;
2877  const btVector3 fv=vrel-iv;
2878  impulse.m_velocity += iv+fv*m_friction;
2879  }
2880  impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
2881 
2882  if (m_bodies[0].m_soft==m_bodies[1].m_soft)
2883  {
2884  if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
2885  (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
2886  {
2887  if (impulse.m_asVelocity)
2888  {
2889  if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
2890  {
2891 
2892  } else
2893  {
2894  m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
2895  m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
2896  }
2897  }
2898  }
2899  } else
2900  {
2901  m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2902  m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2903  }
2904 }
2905 
2906 //
2907 void btSoftBody::CJoint::Terminate(btScalar dt)
2908 {
2909  if(m_split>0)
2910  {
2911  m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2912  m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2913  }
2914 }
2915 
2916 //
2917 void btSoftBody::applyForces()
2918 {
2919 
2920  BT_PROFILE("SoftBody applyForces");
2921 // const btScalar dt = m_sst.sdt;
2922  const btScalar kLF = m_cfg.kLF;
2923  const btScalar kDG = m_cfg.kDG;
2924  const btScalar kPR = m_cfg.kPR;
2925  const btScalar kVC = m_cfg.kVC;
2926  const bool as_lift = kLF>0;
2927  const bool as_drag = kDG>0;
2928  const bool as_pressure = kPR!=0;
2929  const bool as_volume = kVC>0;
2930  const bool as_aero = as_lift ||
2931  as_drag ;
2932  //const bool as_vaero = as_aero &&
2933  // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
2934  //const bool as_faero = as_aero &&
2935  // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
2936  const bool use_medium = as_aero;
2937  const bool use_volume = as_pressure ||
2938  as_volume ;
2939  btScalar volume = 0;
2940  btScalar ivolumetp = 0;
2941  btScalar dvolumetv = 0;
2942  btSoftBody::sMedium medium;
2943  if(use_volume)
2944  {
2945  volume = getVolume();
2946  ivolumetp = 1/btFabs(volume)*kPR;
2947  dvolumetv = (m_pose.m_volume-volume)*kVC;
2948  }
2949  /* Per vertex forces */
2950  int i,ni;
2951 
2952  for(i=0,ni=m_nodes.size();i<ni;++i)
2953  {
2954  btSoftBody::Node& n=m_nodes[i];
2955  if(n.m_im>0)
2956  {
2957  if(use_medium)
2958  {
2959  /* Aerodynamics */
2960  addAeroForceToNode(m_windVelocity, i);
2961  }
2962  /* Pressure */
2963  if(as_pressure)
2964  {
2965  n.m_f += n.m_n*(n.m_area*ivolumetp);
2966  }
2967  /* Volume */
2968  if(as_volume)
2969  {
2970  n.m_f += n.m_n*(n.m_area*dvolumetv);
2971  }
2972  }
2973  }
2974 
2975  /* Per face forces */
2976  for(i=0,ni=m_faces.size();i<ni;++i)
2977  {
2978  // btSoftBody::Face& f=m_faces[i];
2979 
2980  /* Aerodynamics */
2981  addAeroForceToFace(m_windVelocity, i);
2982  }
2983 }
2984 
2985 //
2986 void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
2987 {
2988  const btScalar kAHR=psb->m_cfg.kAHR*kst;
2989  const btScalar dt=psb->m_sst.sdt;
2990  for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
2991  {
2992  const Anchor& a=psb->m_anchors[i];
2993  const btTransform& t=a.m_body->getWorldTransform();
2994  Node& n=*a.m_node;
2995  const btVector3 wa=t*a.m_local;
2996  const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
2997  const btVector3 vb=n.m_x-n.m_q;
2998  const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
2999  const btVector3 impulse=a.m_c0*vr*a.m_influence;
3000  n.m_x+=impulse*a.m_c2;
3001  a.m_body->applyImpulse(-impulse,a.m_c1);
3002  }
3003 }
3004 
3005 //
3006 void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
3007 {
3008  const btScalar dt = psb->m_sst.sdt;
3009  const btScalar mrg = psb->getCollisionShape()->getMargin();
3010  for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
3011  {
3012  const RContact& c = psb->m_rcontacts[i];
3013  const sCti& cti = c.m_cti;
3014  btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3015 
3016  const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
3017  const btVector3 vb = c.m_node->m_x-c.m_node->m_q;
3018  const btVector3 vr = vb-va;
3019  const btScalar dn = btDot(vr, cti.m_normal);
3020  if(dn<=SIMD_EPSILON)
3021  {
3022  const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
3023  const btVector3 fv = vr - (cti.m_normal * dn);
3024  // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3025  const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
3026  c.m_node->m_x -= impulse * c.m_c2;
3027  if (tmpRigid)
3028  tmpRigid->applyImpulse(impulse,c.m_c1);
3029  }
3030  }
3031 }
3032 
3033 //
3034 void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
3035 {
3036  for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
3037  {
3038  const SContact& c=psb->m_scontacts[i];
3039  const btVector3& nr=c.m_normal;
3040  Node& n=*c.m_node;
3041  Face& f=*c.m_face;
3042  const btVector3 p=BaryEval( f.m_n[0]->m_x,
3043  f.m_n[1]->m_x,
3044  f.m_n[2]->m_x,
3045  c.m_weights);
3046  const btVector3 q=BaryEval( f.m_n[0]->m_q,
3047  f.m_n[1]->m_q,
3048  f.m_n[2]->m_q,
3049  c.m_weights);
3050  const btVector3 vr=(n.m_x-n.m_q)-(p-q);
3051  btVector3 corr(0,0,0);
3052  btScalar dot = btDot(vr,nr);
3053  if(dot<0)
3054  {
3055  const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
3056  corr+=c.m_normal*j;
3057  }
3058  corr -= ProjectOnPlane(vr,nr)*c.m_friction;
3059  n.m_x += corr*c.m_cfm[0];
3060  f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x());
3061  f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y());
3062  f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z());
3063  }
3064 }
3065 
3066 //
3067 void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
3068 {
3069  for(int i=0,ni=psb->m_links.size();i<ni;++i)
3070  {
3071  Link& l=psb->m_links[i];
3072  if(l.m_c0>0)
3073  {
3074  Node& a=*l.m_n[0];
3075  Node& b=*l.m_n[1];
3076  const btVector3 del=b.m_x-a.m_x;
3077  const btScalar len=del.length2();
3078  if (l.m_c1+len > SIMD_EPSILON)
3079  {
3080  const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
3081  a.m_x-=del*(k*a.m_im);
3082  b.m_x+=del*(k*b.m_im);
3083  }
3084  }
3085  }
3086 }
3087 
3088 //
3089 void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
3090 {
3091  for(int i=0,ni=psb->m_links.size();i<ni;++i)
3092  {
3093  Link& l=psb->m_links[i];
3094  Node** n=l.m_n;
3095  const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
3096  n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
3097  n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
3098  }
3099 }
3100 
3101 //
3102 btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
3103 {
3104  switch(solver)
3105  {
3106  case ePSolver::Anchors:
3107  return(&btSoftBody::PSolve_Anchors);
3108  case ePSolver::Linear:
3109  return(&btSoftBody::PSolve_Links);
3110  case ePSolver::RContacts:
3111  return(&btSoftBody::PSolve_RContacts);
3112  case ePSolver::SContacts:
3113  return(&btSoftBody::PSolve_SContacts);
3114  default:
3115  {
3116  }
3117  }
3118  return(0);
3119 }
3120 
3121 //
3122 btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
3123 {
3124  switch(solver)
3125  {
3126  case eVSolver::Linear: return(&btSoftBody::VSolve_Links);
3127  default:
3128  {
3129  }
3130  }
3131  return(0);
3132 }
3133 
3134 //
3135 void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
3136 {
3137 
3138  switch(m_cfg.collisions&fCollision::RVSmask)
3139  {
3140  case fCollision::SDF_RS:
3141  {
3142  btSoftColliders::CollideSDF_RS docollide;
3143  btRigidBody* prb1=(btRigidBody*) btRigidBody::upcast(pcoWrap->getCollisionObject());
3144  btTransform wtr=pcoWrap->getWorldTransform();
3145 
3146  const btTransform ctr=pcoWrap->getWorldTransform();
3147  const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
3148  const btScalar basemargin=getCollisionShape()->getMargin();
3149  btVector3 mins;
3150  btVector3 maxs;
3151  ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
3152  pcoWrap->getCollisionShape()->getAabb( pcoWrap->getWorldTransform(),
3153  mins,
3154  maxs);
3155  volume=btDbvtVolume::FromMM(mins,maxs);
3156  volume.Expand(btVector3(basemargin,basemargin,basemargin));
3157  docollide.psb = this;
3158  docollide.m_colObj1Wrap = pcoWrap;
3159  docollide.m_rigidBody = prb1;
3160 
3161  docollide.dynmargin = basemargin+timemargin;
3162  docollide.stamargin = basemargin;
3163  m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
3164  }
3165  break;
3166  case fCollision::CL_RS:
3167  {
3168  btSoftColliders::CollideCL_RS collider;
3169  collider.ProcessColObj(this,pcoWrap);
3170  }
3171  break;
3172  }
3173 }
3174 
3175 //
3176 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
3177 {
3178  const int cf=m_cfg.collisions&psb->m_cfg.collisions;
3179  switch(cf&fCollision::SVSmask)
3180  {
3181  case fCollision::CL_SS:
3182  {
3183 
3184  //support self-collision if CL_SELF flag set
3185  if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
3186  {
3187  btSoftColliders::CollideCL_SS docollide;
3188  docollide.ProcessSoftSoft(this,psb);
3189  }
3190 
3191  }
3192  break;
3193  case fCollision::VF_SS:
3194  {
3195  //only self-collision for Cluster, not Vertex-Face yet
3196  if (this!=psb)
3197  {
3198  btSoftColliders::CollideVF_SS docollide;
3199  /* common */
3200  docollide.mrg= getCollisionShape()->getMargin()+
3201  psb->getCollisionShape()->getMargin();
3202  /* psb0 nodes vs psb1 faces */
3203  docollide.psb[0]=this;
3204  docollide.psb[1]=psb;
3205  docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
3206  docollide.psb[1]->m_fdbvt.m_root,
3207  docollide);
3208  /* psb1 nodes vs psb0 faces */
3209  docollide.psb[0]=psb;
3210  docollide.psb[1]=this;
3211  docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
3212  docollide.psb[1]->m_fdbvt.m_root,
3213  docollide);
3214  }
3215  }
3216  break;
3217  default:
3218  {
3219 
3220  }
3221  }
3222 }
3223 
3224 
3225 
3226 void btSoftBody::setWindVelocity( const btVector3 &velocity )
3227 {
3228  m_windVelocity = velocity;
3229 }
3230 
3231 
3232 const btVector3& btSoftBody::getWindVelocity()
3233 {
3234  return m_windVelocity;
3235 }
3236 
3237 
3238 
3239 int btSoftBody::calculateSerializeBufferSize() const
3240 {
3241  int sz = sizeof(btSoftBodyData);
3242  return sz;
3243 }
3244 
3246 const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
3247 {
3248  btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer;
3249 
3250  btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
3251 
3252  btHashMap<btHashPtr,int> m_nodeIndexMap;
3253 
3254  sbd->m_numMaterials = m_materials.size();
3255  sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0;
3256 
3257  if (sbd->m_materials)
3258  {
3259  int sz = sizeof(SoftBodyMaterialData*);
3260  int numElem = sbd->m_numMaterials;
3261  btChunk* chunk = serializer->allocate(sz,numElem);
3262  //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
3263  SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
3264  for (int i=0;i<numElem;i++,memPtr++)
3265  {
3266  btSoftBody::Material* mat = m_materials[i];
3267  *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
3268  if (!serializer->findPointer(mat))
3269  {
3270  //serialize it here
3271  btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1);
3272  SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
3273  memPtr->m_flags = mat->m_flags;
3274  memPtr->m_angularStiffness = mat->m_kAST;
3275  memPtr->m_linearStiffness = mat->m_kLST;
3276  memPtr->m_volumeStiffness = mat->m_kVST;
3277  serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat);
3278  }
3279  }
3280  serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials);
3281  }
3282 
3283 
3284 
3285 
3286  sbd->m_numNodes = m_nodes.size();
3287  sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0;
3288  if (sbd->m_nodes)
3289  {
3290  int sz = sizeof(SoftBodyNodeData);
3291  int numElem = sbd->m_numNodes;
3292  btChunk* chunk = serializer->allocate(sz,numElem);
3293  SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
3294  for (int i=0;i<numElem;i++,memPtr++)
3295  {
3296  m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce);
3297  memPtr->m_area = m_nodes[i].m_area;
3298  memPtr->m_attach = m_nodes[i].m_battach;
3299  memPtr->m_inverseMass = m_nodes[i].m_im;
3300  memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0;
3301  m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
3302  m_nodes[i].m_x.serializeFloat(memPtr->m_position);
3303  m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
3304  m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
3305  m_nodeIndexMap.insert(&m_nodes[i],i);
3306  }
3307  serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes);
3308  }
3309 
3310  sbd->m_numLinks = m_links.size();
3311  sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
3312  if (sbd->m_links)
3313  {
3314  int sz = sizeof(SoftBodyLinkData);
3315  int numElem = sbd->m_numLinks;
3316  btChunk* chunk = serializer->allocate(sz,numElem);
3317  SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
3318  for (int i=0;i<numElem;i++,memPtr++)
3319  {
3320  memPtr->m_bbending = m_links[i].m_bbending;
3321  memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0;
3322  memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1;
3323  memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1;
3324  btAssert(memPtr->m_nodeIndices[0]<m_nodes.size());
3325  btAssert(memPtr->m_nodeIndices[1]<m_nodes.size());
3326  memPtr->m_restLength = m_links[i].m_rl;
3327  }
3328  serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]);
3329 
3330  }
3331 
3332 
3333  sbd->m_numFaces = m_faces.size();
3334  sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0;
3335  if (sbd->m_faces)
3336  {
3337  int sz = sizeof(SoftBodyFaceData);
3338  int numElem = sbd->m_numFaces;
3339  btChunk* chunk = serializer->allocate(sz,numElem);
3340  SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
3341  for (int i=0;i<numElem;i++,memPtr++)
3342  {
3343  memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0;
3344  m_faces[i].m_normal.serializeFloat( memPtr->m_normal);
3345  for (int j=0;j<3;j++)
3346  {
3347  memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1;
3348  }
3349  memPtr->m_restArea = m_faces[i].m_ra;
3350  }
3351  serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]);
3352  }
3353 
3354 
3355  sbd->m_numTetrahedra = m_tetras.size();
3356  sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0;
3357  if (sbd->m_tetrahedra)
3358  {
3359  int sz = sizeof(SoftBodyTetraData);
3360  int numElem = sbd->m_numTetrahedra;
3361  btChunk* chunk = serializer->allocate(sz,numElem);
3362  SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
3363  for (int i=0;i<numElem;i++,memPtr++)
3364  {
3365  for (int j=0;j<4;j++)
3366  {
3367  m_tetras[i].m_c0[j].serializeFloat( memPtr->m_c0[j] );
3368  memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1;
3369  }
3370  memPtr->m_c1 = m_tetras[i].m_c1;
3371  memPtr->m_c2 = m_tetras[i].m_c2;
3372  memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0;
3373  memPtr->m_restVolume = m_tetras[i].m_rv;
3374  }
3375  serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]);
3376  }
3377 
3378  sbd->m_numAnchors = m_anchors.size();
3379  sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0;
3380  if (sbd->m_anchors)
3381  {
3382  int sz = sizeof(SoftRigidAnchorData);
3383  int numElem = sbd->m_numAnchors;
3384  btChunk* chunk = serializer->allocate(sz,numElem);
3385  SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
3386  for (int i=0;i<numElem;i++,memPtr++)
3387  {
3388  m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
3389  m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
3390  memPtr->m_c2 = m_anchors[i].m_c2;
3391  m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
3392  memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1;
3393 
3394  memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*) serializer->getUniquePointer((void*)m_anchors[i].m_body): 0;
3395  btAssert(memPtr->m_nodeIndex < m_nodes.size());
3396  }
3397  serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]);
3398  }
3399 
3400 
3401  sbd->m_config.m_dynamicFriction = m_cfg.kDF;
3402  sbd->m_config.m_baumgarte = m_cfg.kVCF;
3403  sbd->m_config.m_pressure = m_cfg.kPR;
3404  sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
3405  sbd->m_config.m_lift = m_cfg.kLF;
3406  sbd->m_config.m_drag = m_cfg.kDG;
3407  sbd->m_config.m_positionIterations = m_cfg.piterations;
3408  sbd->m_config.m_driftIterations = m_cfg.diterations;
3409  sbd->m_config.m_clusterIterations = m_cfg.citerations;
3410  sbd->m_config.m_velocityIterations = m_cfg.viterations;
3411  sbd->m_config.m_maxVolume = m_cfg.maxvolume;
3412  sbd->m_config.m_damping = m_cfg.kDP;
3413  sbd->m_config.m_poseMatch = m_cfg.kMT;
3414  sbd->m_config.m_collisionFlags = m_cfg.collisions;
3415  sbd->m_config.m_volume = m_cfg.kVC;
3416  sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
3417  sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
3418  sbd->m_config.m_softContactHardness = m_cfg.kSHR;
3419  sbd->m_config.m_anchorHardness = m_cfg.kAHR;
3420  sbd->m_config.m_timeScale = m_cfg.timescale;
3421  sbd->m_config.m_maxVolume = m_cfg.maxvolume;
3422  sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
3423  sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
3424  sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
3425  sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
3426  sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
3427  sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
3428 
3429  //pose for shape matching
3430  {
3431  sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
3432 
3433  int sz = sizeof(SoftBodyPoseData);
3434  btChunk* chunk = serializer->allocate(sz,1);
3435  SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
3436 
3437  m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
3438  memPtr->m_bframe = m_pose.m_bframe;
3439  memPtr->m_bvolume = m_pose.m_bvolume;
3440  m_pose.m_com.serializeFloat(memPtr->m_com);
3441 
3442  memPtr->m_numPositions = m_pose.m_pos.size();
3443  memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0;
3444  if (memPtr->m_numPositions)
3445  {
3446  int numElem = memPtr->m_numPositions;
3447  int sz = sizeof(btVector3Data);
3448  btChunk* chunk = serializer->allocate(sz,numElem);
3449  btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
3450  for (int i=0;i<numElem;i++,memPtr++)
3451  {
3452  m_pose.m_pos[i].serializeFloat(*memPtr);
3453  }
3454  serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]);
3455  }
3456  memPtr->m_restVolume = m_pose.m_volume;
3457  m_pose.m_rot.serializeFloat(memPtr->m_rot);
3458  m_pose.m_scl.serializeFloat(memPtr->m_scale);
3459 
3460  memPtr->m_numWeigts = m_pose.m_wgh.size();
3461  memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0;
3462  if (memPtr->m_numWeigts)
3463  {
3464 
3465  int numElem = memPtr->m_numWeigts;
3466  int sz = sizeof(float);
3467  btChunk* chunk = serializer->allocate(sz,numElem);
3468  float* memPtr = (float*) chunk->m_oldPtr;
3469  for (int i=0;i<numElem;i++,memPtr++)
3470  {
3471  *memPtr = m_pose.m_wgh[i];
3472  }
3473  serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]);
3474  }
3475 
3476  serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose);
3477  }
3478 
3479  //clusters for convex-cluster collision detection
3480 
3481  sbd->m_numClusters = m_clusters.size();
3482  sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0;
3483  if (sbd->m_numClusters)
3484  {
3485  int numElem = sbd->m_numClusters;
3486  int sz = sizeof(SoftBodyClusterData);
3487  btChunk* chunk = serializer->allocate(sz,numElem);
3488  SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
3489  for (int i=0;i<numElem;i++,memPtr++)
3490  {
3491  memPtr->m_adamping= m_clusters[i]->m_adamping;
3492  m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
3493  memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
3494  memPtr->m_collide = m_clusters[i]->m_collide;
3495  m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
3496  memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
3497  m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
3498  m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
3499  m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
3500  memPtr->m_idmass = m_clusters[i]->m_idmass;
3501  memPtr->m_imass = m_clusters[i]->m_imass;
3502  m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
3503  memPtr->m_ldamping = m_clusters[i]->m_ldamping;
3504  m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
3505  m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
3506  memPtr->m_matching = m_clusters[i]->m_matching;
3507  memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
3508  memPtr->m_ndamping = m_clusters[i]->m_ndamping;
3509  memPtr->m_ldamping = m_clusters[i]->m_ldamping;
3510  memPtr->m_adamping = m_clusters[i]->m_adamping;
3511  memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
3512 
3513  memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
3514  memPtr->m_numMasses = m_clusters[i]->m_masses.size();
3515  memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
3516 
3517  memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
3518  m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
3519  m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
3520  memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
3521 
3522 
3523 
3524  memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
3525  if (memPtr->m_framerefs)
3526  {
3527  int numElem = memPtr->m_numFrameRefs;
3528  int sz = sizeof(btVector3FloatData);
3529  btChunk* chunk = serializer->allocate(sz,numElem);
3530  btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
3531  for (int j=0;j<numElem;j++,memPtr++)
3532  {
3533  m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
3534  }
3535  serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]);
3536  }
3537 
3538  memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0;
3539  if (memPtr->m_masses)
3540  {
3541  int numElem = memPtr->m_numMasses;
3542  int sz = sizeof(float);
3543  btChunk* chunk = serializer->allocate(sz,numElem);
3544  float* memPtr = (float*) chunk->m_oldPtr;
3545  for (int j=0;j<numElem;j++,memPtr++)
3546  {
3547  *memPtr = m_clusters[i]->m_masses[j];
3548  }
3549  serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]);
3550  }
3551 
3552  memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0;
3553  if (memPtr->m_nodeIndices )
3554  {
3555  int numElem = memPtr->m_numMasses;
3556  int sz = sizeof(int);
3557  btChunk* chunk = serializer->allocate(sz,numElem);
3558  int* memPtr = (int*) chunk->m_oldPtr;
3559  for (int j=0;j<numElem;j++,memPtr++)
3560  {
3561  int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
3562  btAssert(indexPtr);
3563  *memPtr = *indexPtr;
3564  }
3565  serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes);
3566  }
3567  }
3568  serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]);
3569 
3570  }
3571 
3572 
3573 
3574  sbd->m_numJoints = m_joints.size();
3575  sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0;
3576 
3577  if (sbd->m_joints)
3578  {
3579  int sz = sizeof(btSoftBodyJointData);
3580  int numElem = m_joints.size();
3581  btChunk* chunk = serializer->allocate(sz,numElem);
3582  btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
3583 
3584  for (int i=0;i<numElem;i++,memPtr++)
3585  {
3586  memPtr->m_jointType = (int)m_joints[i]->Type();
3587  m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
3588  m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
3589  memPtr->m_cfm = m_joints[i]->m_cfm;
3590  memPtr->m_erp = m_joints[i]->m_erp;
3591  memPtr->m_split = m_joints[i]->m_split;
3592  memPtr->m_delete = m_joints[i]->m_delete;
3593 
3594  for (int j=0;j<4;j++)
3595  {
3596  memPtr->m_relPosition[0].m_floats[j] = 0.f;
3597  memPtr->m_relPosition[1].m_floats[j] = 0.f;
3598  }
3599  memPtr->m_bodyA = 0;
3600  memPtr->m_bodyB = 0;
3601  if (m_joints[i]->m_bodies[0].m_soft)
3602  {
3603  memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
3604  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
3605  }
3606  if (m_joints[i]->m_bodies[0].m_collisionObject)
3607  {
3608  memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
3609  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
3610  }
3611  if (m_joints[i]->m_bodies[0].m_rigid)
3612  {
3613  memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
3614  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
3615  }
3616 
3617  if (m_joints[i]->m_bodies[1].m_soft)
3618  {
3619  memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
3620  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
3621  }
3622  if (m_joints[i]->m_bodies[1].m_collisionObject)
3623  {
3624  memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
3625  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
3626  }
3627  if (m_joints[i]->m_bodies[1].m_rigid)
3628  {
3629  memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
3630  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
3631  }
3632  }
3633  serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]);
3634  }
3635 
3636 
3637  return btSoftBodyDataName;
3638 }
3639 
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4