Bullet Collision Detection & Physics Library
btSoftBodyHelpers.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 <stdio.h>
19 #include <string.h>
20 #include "btSoftBodyHelpers.h"
21 #include "LinearMath/btConvexHull.h"
22 #include "LinearMath/btConvexHullComputer.h"
23 
24 
25 //
26 static void drawVertex( btIDebugDraw* idraw,
27  const btVector3& x,btScalar s,const btVector3& c)
28 {
29  idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
30  idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
31  idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
32 }
33 
34 //
35 static void drawBox( btIDebugDraw* idraw,
36  const btVector3& mins,
37  const btVector3& maxs,
38  const btVector3& color)
39 {
40  const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()),
41  btVector3(maxs.x(),mins.y(),mins.z()),
42  btVector3(maxs.x(),maxs.y(),mins.z()),
43  btVector3(mins.x(),maxs.y(),mins.z()),
44  btVector3(mins.x(),mins.y(),maxs.z()),
45  btVector3(maxs.x(),mins.y(),maxs.z()),
46  btVector3(maxs.x(),maxs.y(),maxs.z()),
47  btVector3(mins.x(),maxs.y(),maxs.z())};
48  idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
49  idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
50  idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
51  idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
52  idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
53  idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
54 }
55 
56 //
57 static void drawTree( btIDebugDraw* idraw,
58  const btDbvtNode* node,
59  int depth,
60  const btVector3& ncolor,
61  const btVector3& lcolor,
62  int mindepth,
63  int maxdepth)
64 {
65  if(node)
66  {
67  if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
68  {
69  drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
70  drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
71  }
72  if(depth>=mindepth)
73  {
74  const btScalar scl=(btScalar)(node->isinternal()?1:1);
75  const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;
76  const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;
77  drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
78  }
79  }
80 }
81 
82 //
83 template <typename T>
84 static inline T sum(const btAlignedObjectArray<T>& items)
85 {
86  T v;
87  if(items.size())
88  {
89  v=items[0];
90  for(int i=1,ni=items.size();i<ni;++i)
91  {
92  v+=items[i];
93  }
94  }
95  return(v);
96 }
97 
98 //
99 template <typename T,typename Q>
100 static inline void add(btAlignedObjectArray<T>& items,const Q& value)
101 {
102  for(int i=0,ni=items.size();i<ni;++i)
103  {
104  items[i]+=value;
105  }
106 }
107 
108 //
109 template <typename T,typename Q>
110 static inline void mul(btAlignedObjectArray<T>& items,const Q& value)
111 {
112  for(int i=0,ni=items.size();i<ni;++i)
113  {
114  items[i]*=value;
115  }
116 }
117 
118 //
119 template <typename T>
120 static inline T average(const btAlignedObjectArray<T>& items)
121 {
122  const btScalar n=(btScalar)(items.size()>0?items.size():1);
123  return(sum(items)/n);
124 }
125 
126 //
127 static inline btScalar tetravolume(const btVector3& x0,
128  const btVector3& x1,
129  const btVector3& x2,
130  const btVector3& x3)
131 {
132  const btVector3 a=x1-x0;
133  const btVector3 b=x2-x0;
134  const btVector3 c=x3-x0;
135  return(btDot(a,btCross(b,c)));
136 }
137 
138 //
139 #if 0
140 static btVector3 stresscolor(btScalar stress)
141 {
142  static const btVector3 spectrum[]= { btVector3(1,0,1),
143  btVector3(0,0,1),
144  btVector3(0,1,1),
145  btVector3(0,1,0),
146  btVector3(1,1,0),
147  btVector3(1,0,0),
148  btVector3(1,0,0)};
149  static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
150  static const btScalar one=1;
151  stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
152  const int sel=(int)stress;
153  const btScalar frc=stress-sel;
154  return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
155 }
156 #endif
157 
158 //
159 void btSoftBodyHelpers::Draw( btSoftBody* psb,
160  btIDebugDraw* idraw,
161  int drawflags)
162 {
163  const btScalar scl=(btScalar)0.1;
164  const btScalar nscl=scl*5;
165  const btVector3 lcolor=btVector3(0,0,0);
166  const btVector3 ncolor=btVector3(1,1,1);
167  const btVector3 ccolor=btVector3(1,0,0);
168  int i,j,nj;
169 
170  /* Clusters */
171  if(0!=(drawflags&fDrawFlags::Clusters))
172  {
173  srand(1806);
174  for(i=0;i<psb->m_clusters.size();++i)
175  {
176  if(psb->m_clusters[i]->m_collide)
177  {
178  btVector3 color( rand()/(btScalar)RAND_MAX,
179  rand()/(btScalar)RAND_MAX,
180  rand()/(btScalar)RAND_MAX);
181  color=color.normalized()*0.75;
182  btAlignedObjectArray<btVector3> vertices;
183  vertices.resize(psb->m_clusters[i]->m_nodes.size());
184  for(j=0,nj=vertices.size();j<nj;++j)
185  {
186  vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
187  }
188 #define USE_NEW_CONVEX_HULL_COMPUTER
189 #ifdef USE_NEW_CONVEX_HULL_COMPUTER
190  btConvexHullComputer computer;
191  int stride = sizeof(btVector3);
192  int count = vertices.size();
193  btScalar shrink=0.f;
194  btScalar shrinkClamp=0.f;
195  computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp);
196  for (int i=0;i<computer.faces.size();i++)
197  {
198 
199  int face = computer.faces[i];
200  //printf("face=%d\n",face);
201  const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];
202  const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();
203 
204  int v0 = firstEdge->getSourceVertex();
205  int v1 = firstEdge->getTargetVertex();
206  while (edge!=firstEdge)
207  {
208  int v2 = edge->getTargetVertex();
209  idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1);
210  edge = edge->getNextEdgeOfFace();
211  v0=v1;
212  v1=v2;
213  };
214  }
215 #else
216 
217  HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
218  HullResult hres;
219  HullLibrary hlib;
220  hdsc.mMaxVertices=vertices.size();
221  hlib.CreateConvexHull(hdsc,hres);
222  const btVector3 center=average(hres.m_OutputVertices);
223  add(hres.m_OutputVertices,-center);
224  mul(hres.m_OutputVertices,(btScalar)1);
225  add(hres.m_OutputVertices,center);
226  for(j=0;j<(int)hres.mNumFaces;++j)
227  {
228  const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
229  idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
230  hres.m_OutputVertices[idx[1]],
231  hres.m_OutputVertices[idx[2]],
232  color,1);
233  }
234  hlib.ReleaseResult(hres);
235 #endif
236 
237  }
238  /* Velocities */
239 #if 0
240  for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
241  {
242  const btSoftBody::Cluster& c=psb->m_clusters[i];
243  const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
244  const btVector3 v=c.m_lv+btCross(c.m_av,r);
245  idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
246  }
247 #endif
248  /* Frame */
249  // btSoftBody::Cluster& c=*psb->m_clusters[i];
250  // idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
251  // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
252  // idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
253  }
254  }
255  else
256  {
257  /* Nodes */
258  if(0!=(drawflags&fDrawFlags::Nodes))
259  {
260  for(i=0;i<psb->m_nodes.size();++i)
261  {
262  const btSoftBody::Node& n=psb->m_nodes[i];
263  if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
264  idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
265  idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
266  idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
267  }
268  }
269  /* Links */
270  if(0!=(drawflags&fDrawFlags::Links))
271  {
272  for(i=0;i<psb->m_links.size();++i)
273  {
274  const btSoftBody::Link& l=psb->m_links[i];
275  if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
276  idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
277  }
278  }
279  /* Normals */
280  if(0!=(drawflags&fDrawFlags::Normals))
281  {
282  for(i=0;i<psb->m_nodes.size();++i)
283  {
284  const btSoftBody::Node& n=psb->m_nodes[i];
285  if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
286  const btVector3 d=n.m_n*nscl;
287  idraw->drawLine(n.m_x,n.m_x+d,ncolor);
288  idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
289  }
290  }
291  /* Contacts */
292  if(0!=(drawflags&fDrawFlags::Contacts))
293  {
294  static const btVector3 axis[]={btVector3(1,0,0),
295  btVector3(0,1,0),
296  btVector3(0,0,1)};
297  for(i=0;i<psb->m_rcontacts.size();++i)
298  {
299  const btSoftBody::RContact& c=psb->m_rcontacts[i];
300  const btVector3 o= c.m_node->m_x-c.m_cti.m_normal*
301  (btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
302  const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
303  const btVector3 y=btCross(x,c.m_cti.m_normal).normalized();
304  idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
305  idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
306  idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
307  }
308  }
309  /* Faces */
310  if(0!=(drawflags&fDrawFlags::Faces))
311  {
312  const btScalar scl=(btScalar)0.8;
313  const btScalar alp=(btScalar)1;
314  const btVector3 col(0,(btScalar)0.7,0);
315  for(i=0;i<psb->m_faces.size();++i)
316  {
317  const btSoftBody::Face& f=psb->m_faces[i];
318  if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
319  const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
320  const btVector3 c=(x[0]+x[1]+x[2])/3;
321  idraw->drawTriangle((x[0]-c)*scl+c,
322  (x[1]-c)*scl+c,
323  (x[2]-c)*scl+c,
324  col,alp);
325  }
326  }
327  /* Tetras */
328  if(0!=(drawflags&fDrawFlags::Tetras))
329  {
330  const btScalar scl=(btScalar)0.8;
331  const btScalar alp=(btScalar)1;
332  const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7);
333  for(int i=0;i<psb->m_tetras.size();++i)
334  {
335  const btSoftBody::Tetra& t=psb->m_tetras[i];
336  if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
337  const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};
338  const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4;
339  idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
340  idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
341  idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
342  idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
343  }
344  }
345  }
346  /* Anchors */
347  if(0!=(drawflags&fDrawFlags::Anchors))
348  {
349  for(i=0;i<psb->m_anchors.size();++i)
350  {
351  const btSoftBody::Anchor& a=psb->m_anchors[i];
352  const btVector3 q=a.m_body->getWorldTransform()*a.m_local;
353  drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
354  drawVertex(idraw,q,0.25,btVector3(0,1,0));
355  idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
356  }
357  for(i=0;i<psb->m_nodes.size();++i)
358  {
359  const btSoftBody::Node& n=psb->m_nodes[i];
360  if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
361  if(n.m_im<=0)
362  {
363  drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
364  }
365  }
366  }
367 
368 
369  /* Notes */
370  if(0!=(drawflags&fDrawFlags::Notes))
371  {
372  for(i=0;i<psb->m_notes.size();++i)
373  {
374  const btSoftBody::Note& n=psb->m_notes[i];
375  btVector3 p=n.m_offset;
376  for(int j=0;j<n.m_rank;++j)
377  {
378  p+=n.m_nodes[j]->m_x*n.m_coords[j];
379  }
380  idraw->draw3dText(p,n.m_text);
381  }
382  }
383  /* Node tree */
384  if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw);
385  /* Face tree */
386  if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw);
387  /* Cluster tree */
388  if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw);
389  /* Joints */
390  if(0!=(drawflags&fDrawFlags::Joints))
391  {
392  for(i=0;i<psb->m_joints.size();++i)
393  {
394  const btSoftBody::Joint* pj=psb->m_joints[i];
395  switch(pj->Type())
396  {
397  case btSoftBody::Joint::eType::Linear:
398  {
399  const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj;
400  const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
401  const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
402  idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
403  idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
404  drawVertex(idraw,a0,0.25,btVector3(1,1,0));
405  drawVertex(idraw,a1,0.25,btVector3(0,1,1));
406  }
407  break;
408  case btSoftBody::Joint::eType::Angular:
409  {
410  //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
411  const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
412  const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
413  const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
414  const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
415  idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
416  idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
417  idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
418  idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
419  break;
420  }
421  default:
422  {
423  }
424 
425  }
426  }
427  }
428 }
429 
430 //
431 void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
432  btIDebugDraw* idraw,
433  bool masses,
434  bool areas,
435  bool /*stress*/)
436 {
437  for(int i=0;i<psb->m_nodes.size();++i)
438  {
439  const btSoftBody::Node& n=psb->m_nodes[i];
440  char text[2048]={0};
441  char buff[1024];
442  if(masses)
443  {
444  sprintf(buff," M(%.2f)",1/n.m_im);
445  strcat(text,buff);
446  }
447  if(areas)
448  {
449  sprintf(buff," A(%.2f)",n.m_area);
450  strcat(text,buff);
451  }
452  if(text[0]) idraw->draw3dText(n.m_x,text);
453  }
454 }
455 
456 //
457 void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb,
458  btIDebugDraw* idraw,
459  int mindepth,
460  int maxdepth)
461 {
462  drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
463 }
464 
465 //
466 void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb,
467  btIDebugDraw* idraw,
468  int mindepth,
469  int maxdepth)
470 {
471  drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
472 }
473 
474 //
475 void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb,
476  btIDebugDraw* idraw,
477  int mindepth,
478  int maxdepth)
479 {
480  drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
481 }
482 
483 //
484 void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
485  btIDebugDraw* idraw)
486 {
487  if(psb->m_pose.m_bframe)
488  {
489  static const btScalar ascl=10;
490  static const btScalar nscl=(btScalar)0.1;
491  const btVector3 com=psb->m_pose.m_com;
492  const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
493  const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized();
494  const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized();
495  const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized();
496  idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
497  idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
498  idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
499  for(int i=0;i<psb->m_pose.m_pos.size();++i)
500  {
501  const btVector3 x=com+trs*psb->m_pose.m_pos[i];
502  drawVertex(idraw,x,nscl,btVector3(1,0,1));
503  }
504  }
505 }
506 
507 //
508 btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from,
509  const btVector3& to,
510  int res,
511  int fixeds)
512 {
513  /* Create nodes */
514  const int r=res+2;
515  btVector3* x=new btVector3[r];
516  btScalar* m=new btScalar[r];
517  int i;
518 
519  for(i=0;i<r;++i)
520  {
521  const btScalar t=i/(btScalar)(r-1);
522  x[i]=lerp(from,to,t);
523  m[i]=1;
524  }
525  btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m);
526  if(fixeds&1) psb->setMass(0,0);
527  if(fixeds&2) psb->setMass(r-1,0);
528  delete[] x;
529  delete[] m;
530  /* Create links */
531  for(i=1;i<r;++i)
532  {
533  psb->appendLink(i-1,i);
534  }
535  /* Finished */
536  return(psb);
537 }
538 
539 //
540 btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
541  const btVector3& corner10,
542  const btVector3& corner01,
543  const btVector3& corner11,
544  int resx,
545  int resy,
546  int fixeds,
547  bool gendiags)
548 {
549 #define IDX(_x_,_y_) ((_y_)*rx+(_x_))
550  /* Create nodes */
551  if((resx<2)||(resy<2)) return(0);
552  const int rx=resx;
553  const int ry=resy;
554  const int tot=rx*ry;
555  btVector3* x=new btVector3[tot];
556  btScalar* m=new btScalar[tot];
557  int iy;
558 
559  for(iy=0;iy<ry;++iy)
560  {
561  const btScalar ty=iy/(btScalar)(ry-1);
562  const btVector3 py0=lerp(corner00,corner01,ty);
563  const btVector3 py1=lerp(corner10,corner11,ty);
564  for(int ix=0;ix<rx;++ix)
565  {
566  const btScalar tx=ix/(btScalar)(rx-1);
567  x[IDX(ix,iy)]=lerp(py0,py1,tx);
568  m[IDX(ix,iy)]=1;
569  }
570  }
571  btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m);
572  if(fixeds&1) psb->setMass(IDX(0,0),0);
573  if(fixeds&2) psb->setMass(IDX(rx-1,0),0);
574  if(fixeds&4) psb->setMass(IDX(0,ry-1),0);
575  if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0);
576  delete[] x;
577  delete[] m;
578  /* Create links and faces */
579  for(iy=0;iy<ry;++iy)
580  {
581  for(int ix=0;ix<rx;++ix)
582  {
583  const int idx=IDX(ix,iy);
584  const bool mdx=(ix+1)<rx;
585  const bool mdy=(iy+1)<ry;
586  if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
587  if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
588  if(mdx&&mdy)
589  {
590  if((ix+iy)&1)
591  {
592  psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
593  psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
594  if(gendiags)
595  {
596  psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
597  }
598  }
599  else
600  {
601  psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
602  psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
603  if(gendiags)
604  {
605  psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
606  }
607  }
608  }
609  }
610  }
611  /* Finished */
612 #undef IDX
613  return(psb);
614 }
615 
616 //
617 btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
618  const btVector3& corner00,
619  const btVector3& corner10,
620  const btVector3& corner01,
621  const btVector3& corner11,
622  int resx,
623  int resy,
624  int fixeds,
625  bool gendiags,
626  float* tex_coords)
627 {
628 
629  /*
630  *
631  * corners:
632  *
633  * [0][0] corner00 ------- corner01 [resx][0]
634  * | |
635  * | |
636  * [0][resy] corner10 -------- corner11 [resx][resy]
637  *
638  *
639  *
640  *
641  *
642  *
643  * "fixedgs" map:
644  *
645  * corner00 --> +1
646  * corner01 --> +2
647  * corner10 --> +4
648  * corner11 --> +8
649  * upper middle --> +16
650  * left middle --> +32
651  * right middle --> +64
652  * lower middle --> +128
653  * center --> +256
654  *
655  *
656  * tex_coords size (resx-1)*(resy-1)*12
657  *
658  *
659  *
660  * SINGLE QUAD INTERNALS
661  *
662  * 1) btSoftBody's nodes and links,
663  * diagonal link is optional ("gendiags")
664  *
665  *
666  * node00 ------ node01
667  * | .
668  * | .
669  * | .
670  * | .
671  * | .
672  * node10 node11
673  *
674  *
675  *
676  * 2) Faces:
677  * two triangles,
678  * UV Coordinates (hier example for single quad)
679  *
680  * (0,1) (0,1) (1,1)
681  * 1 |\ 3 \-----| 2
682  * | \ \ |
683  * | \ \ |
684  * | \ \ |
685  * | \ \ |
686  * 2 |-----\ 3 \| 1
687  * (0,0) (1,0) (1,0)
688  *
689  *
690  *
691  *
692  *
693  *
694  */
695 
696 #define IDX(_x_,_y_) ((_y_)*rx+(_x_))
697  /* Create nodes */
698  if((resx<2)||(resy<2)) return(0);
699  const int rx=resx;
700  const int ry=resy;
701  const int tot=rx*ry;
702  btVector3* x=new btVector3[tot];
703  btScalar* m=new btScalar[tot];
704 
705  int iy;
706 
707  for(iy=0;iy<ry;++iy)
708  {
709  const btScalar ty=iy/(btScalar)(ry-1);
710  const btVector3 py0=lerp(corner00,corner01,ty);
711  const btVector3 py1=lerp(corner10,corner11,ty);
712  for(int ix=0;ix<rx;++ix)
713  {
714  const btScalar tx=ix/(btScalar)(rx-1);
715  x[IDX(ix,iy)]=lerp(py0,py1,tx);
716  m[IDX(ix,iy)]=1;
717  }
718  }
719  btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m);
720  if(fixeds&1) psb->setMass(IDX(0,0),0);
721  if(fixeds&2) psb->setMass(IDX(rx-1,0),0);
722  if(fixeds&4) psb->setMass(IDX(0,ry-1),0);
723  if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0);
724  if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0);
725  if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0);
726  if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0);
727  if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0);
728  if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
729  delete[] x;
730  delete[] m;
731 
732 
733  int z = 0;
734  /* Create links and faces */
735  for(iy=0;iy<ry;++iy)
736  {
737  for(int ix=0;ix<rx;++ix)
738  {
739  const bool mdx=(ix+1)<rx;
740  const bool mdy=(iy+1)<ry;
741 
742  int node00=IDX(ix,iy);
743  int node01=IDX(ix+1,iy);
744  int node10=IDX(ix,iy+1);
745  int node11=IDX(ix+1,iy+1);
746 
747  if(mdx) psb->appendLink(node00,node01);
748  if(mdy) psb->appendLink(node00,node10);
749  if(mdx&&mdy)
750  {
751  psb->appendFace(node00,node10,node11);
752  if (tex_coords) {
753  tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
754  tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
755  tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
756  tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
757  tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
758  tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
759  }
760  psb->appendFace(node11,node01,node00);
761  if (tex_coords) {
762  tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
763  tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
764  tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
765  tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
766  tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
767  tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
768  }
769  if (gendiags) psb->appendLink(node00,node11);
770  z += 12;
771  }
772  }
773  }
774  /* Finished */
775 #undef IDX
776  return(psb);
777 }
778 
779 float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
780 {
781 
782  /*
783  *
784  *
785  * node00 --- node01
786  * | |
787  * node10 --- node11
788  *
789  *
790  * ID map:
791  *
792  * node00 s --> 0
793  * node00 t --> 1
794  *
795  * node01 s --> 3
796  * node01 t --> 1
797  *
798  * node10 s --> 0
799  * node10 t --> 2
800  *
801  * node11 s --> 3
802  * node11 t --> 2
803  *
804  *
805  */
806 
807  float tc=0.0f;
808  if (id == 0) {
809  tc = (1.0f/((resx-1))*ix);
810  }
811  else if (id==1) {
812  tc = (1.0f/((resy-1))*(resy-1-iy));
813  }
814  else if (id==2) {
815  tc = (1.0f/((resy-1))*(resy-1-iy-1));
816  }
817  else if (id==3) {
818  tc = (1.0f/((resx-1))*(ix+1));
819  }
820  return tc;
821 }
822 //
823 btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
824  const btVector3& radius,
825  int res)
826 {
827  struct Hammersley
828  {
829  static void Generate(btVector3* x,int n)
830  {
831  for(int i=0;i<n;i++)
832  {
833  btScalar p=0.5,t=0;
834  for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
835  btScalar w=2*t-1;
836  btScalar a=(SIMD_PI+2*i*SIMD_PI)/n;
837  btScalar s=btSqrt(1-w*w);
838  *x++=btVector3(s*btCos(a),s*btSin(a),w);
839  }
840  }
841  };
842  btAlignedObjectArray<btVector3> vtx;
843  vtx.resize(3+res);
844  Hammersley::Generate(&vtx[0],vtx.size());
845  for(int i=0;i<vtx.size();++i)
846  {
847  vtx[i]=vtx[i]*radius+center;
848  }
849  return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
850 }
851 
852 
853 
854 //
855 btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices,
856  const int* triangles,
857  int ntriangles, bool randomizeConstraints)
858 {
859  int maxidx=0;
860  int i,j,ni;
861 
862  for(i=0,ni=ntriangles*3;i<ni;++i)
863  {
864  maxidx=btMax(triangles[i],maxidx);
865  }
866  ++maxidx;
867  btAlignedObjectArray<bool> chks;
868  btAlignedObjectArray<btVector3> vtx;
869  chks.resize(maxidx*maxidx,false);
870  vtx.resize(maxidx);
871  for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
872  {
873  vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
874  }
875  btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
876  for( i=0,ni=ntriangles*3;i<ni;i+=3)
877  {
878  const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
879 #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
880  for(int j=2,k=0;k<3;j=k++)
881  {
882  if(!chks[IDX(idx[j],idx[k])])
883  {
884  chks[IDX(idx[j],idx[k])]=true;
885  chks[IDX(idx[k],idx[j])]=true;
886  psb->appendLink(idx[j],idx[k]);
887  }
888  }
889 #undef IDX
890  psb->appendFace(idx[0],idx[1],idx[2]);
891  }
892 
893  if (randomizeConstraints)
894  {
895  psb->randomizeConstraints();
896  }
897 
898  return(psb);
899 }
900 
901 //
902 btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,
903  int nvertices, bool randomizeConstraints)
904 {
905  HullDesc hdsc(QF_TRIANGLES,nvertices,vertices);
906  HullResult hres;
907  HullLibrary hlib;/*??*/
908  hdsc.mMaxVertices=nvertices;
909  hlib.CreateConvexHull(hdsc,hres);
910  btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
911  &hres.m_OutputVertices[0],0);
912  for(int i=0;i<(int)hres.mNumFaces;++i)
913  {
914  const int idx[]={ hres.m_Indices[i*3+0],
915  hres.m_Indices[i*3+1],
916  hres.m_Indices[i*3+2]};
917  if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]);
918  if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]);
919  if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]);
920  psb->appendFace(idx[0],idx[1],idx[2]);
921  }
922  hlib.ReleaseResult(hres);
923  if (randomizeConstraints)
924  {
925  psb->randomizeConstraints();
926  }
927  return(psb);
928 }
929 
930 
931 
932 
933 static int nextLine(const char* buffer)
934 {
935  int numBytesRead=0;
936 
937  while (*buffer != '\n')
938  {
939  buffer++;
940  numBytesRead++;
941  }
942 
943 
944  if (buffer[0]==0x0a)
945  {
946  buffer++;
947  numBytesRead++;
948  }
949  return numBytesRead;
950 }
951 
952 /* Create from TetGen .ele, .face, .node data */
953 btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,
954  const char* ele,
955  const char* face,
956  const char* node,
957  bool bfacelinks,
958  bool btetralinks,
959  bool bfacesfromtetras)
960 {
961 btAlignedObjectArray<btVector3> pos;
962 int nnode=0;
963 int ndims=0;
964 int nattrb=0;
965 int hasbounds=0;
966 int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
967 result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
968 node += nextLine(node);
969 
970 pos.resize(nnode);
971 for(int i=0;i<pos.size();++i)
972  {
973  int index=0;
974  //int bound=0;
975  float x,y,z;
976  sscanf(node,"%d %f %f %f",&index,&x,&y,&z);
977 
978 // sn>>index;
979 // sn>>x;sn>>y;sn>>z;
980  node += nextLine(node);
981 
982  //for(int j=0;j<nattrb;++j)
983  // sn>>a;
984 
985  //if(hasbounds)
986  // sn>>bound;
987 
988  pos[index].setX(btScalar(x));
989  pos[index].setY(btScalar(y));
990  pos[index].setZ(btScalar(z));
991  }
992 btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
993 #if 0
994 if(face&&face[0])
995  {
996  int nface=0;
997  sf>>nface;sf>>hasbounds;
998  for(int i=0;i<nface;++i)
999  {
1000  int index=0;
1001  int bound=0;
1002  int ni[3];
1003  sf>>index;
1004  sf>>ni[0];sf>>ni[1];sf>>ni[2];
1005  sf>>bound;
1006  psb->appendFace(ni[0],ni[1],ni[2]);
1007  if(btetralinks)
1008  {
1009  psb->appendLink(ni[0],ni[1],0,true);
1010  psb->appendLink(ni[1],ni[2],0,true);
1011  psb->appendLink(ni[2],ni[0],0,true);
1012  }
1013  }
1014  }
1015 #endif
1016 
1017 if(ele&&ele[0])
1018  {
1019  int ntetra=0;
1020  int ncorner=0;
1021  int neattrb=0;
1022  sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb);
1023  ele += nextLine(ele);
1024 
1025  //se>>ntetra;se>>ncorner;se>>neattrb;
1026  for(int i=0;i<ntetra;++i)
1027  {
1028  int index=0;
1029  int ni[4];
1030 
1031  //se>>index;
1032  //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
1033  sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]);
1034  ele+=nextLine(ele);
1035  //for(int j=0;j<neattrb;++j)
1036  // se>>a;
1037  psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);
1038  if(btetralinks)
1039  {
1040  psb->appendLink(ni[0],ni[1],0,true);
1041  psb->appendLink(ni[1],ni[2],0,true);
1042  psb->appendLink(ni[2],ni[0],0,true);
1043  psb->appendLink(ni[0],ni[3],0,true);
1044  psb->appendLink(ni[1],ni[3],0,true);
1045  psb->appendLink(ni[2],ni[3],0,true);
1046  }
1047  }
1048  }
1049 printf("Nodes: %u\r\n",psb->m_nodes.size());
1050 printf("Links: %u\r\n",psb->m_links.size());
1051 printf("Faces: %u\r\n",psb->m_faces.size());
1052 printf("Tetras: %u\r\n",psb->m_tetras.size());
1053 return(psb);
1054 }
1055 
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4