[1963] | 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 | */ |
---|
| 15 | ///btSoftBodyHelpers.cpp by Nathanael Presson |
---|
| 16 | |
---|
| 17 | #include "btSoftBodyInternals.h" |
---|
| 18 | #include <stdio.h> |
---|
| 19 | #include <string.h> |
---|
| 20 | #include "btSoftBodyHelpers.h" |
---|
| 21 | #include "LinearMath/btConvexHull.h" |
---|
| 22 | |
---|
| 23 | // |
---|
| 24 | static void drawVertex( btIDebugDraw* idraw, |
---|
[1972] | 25 | const btVector3& x,btScalar s,const btVector3& c) |
---|
| 26 | { |
---|
| 27 | idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); |
---|
| 28 | idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); |
---|
| 29 | idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); |
---|
| 30 | } |
---|
[1963] | 31 | |
---|
| 32 | // |
---|
| 33 | static void drawBox( btIDebugDraw* idraw, |
---|
[1972] | 34 | const btVector3& mins, |
---|
| 35 | const btVector3& maxs, |
---|
| 36 | const btVector3& color) |
---|
[1963] | 37 | { |
---|
[1972] | 38 | const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), |
---|
| 39 | btVector3(maxs.x(),mins.y(),mins.z()), |
---|
| 40 | btVector3(maxs.x(),maxs.y(),mins.z()), |
---|
| 41 | btVector3(mins.x(),maxs.y(),mins.z()), |
---|
| 42 | btVector3(mins.x(),mins.y(),maxs.z()), |
---|
| 43 | btVector3(maxs.x(),mins.y(),maxs.z()), |
---|
| 44 | btVector3(maxs.x(),maxs.y(),maxs.z()), |
---|
| 45 | btVector3(mins.x(),maxs.y(),maxs.z())}; |
---|
| 46 | idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); |
---|
| 47 | idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); |
---|
| 48 | idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); |
---|
| 49 | idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); |
---|
| 50 | idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); |
---|
| 51 | idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); |
---|
[1963] | 52 | } |
---|
| 53 | |
---|
| 54 | // |
---|
| 55 | static void drawTree( btIDebugDraw* idraw, |
---|
[1972] | 56 | const btDbvtNode* node, |
---|
| 57 | int depth, |
---|
| 58 | const btVector3& ncolor, |
---|
| 59 | const btVector3& lcolor, |
---|
| 60 | int mindepth, |
---|
| 61 | int maxdepth) |
---|
[1963] | 62 | { |
---|
[1972] | 63 | if(node) |
---|
[1963] | 64 | { |
---|
[1972] | 65 | if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) |
---|
[1963] | 66 | { |
---|
[1972] | 67 | drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); |
---|
| 68 | drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); |
---|
[1963] | 69 | } |
---|
[1972] | 70 | if(depth>=mindepth) |
---|
[1963] | 71 | { |
---|
[1972] | 72 | const btScalar scl=(btScalar)(node->isinternal()?1:1); |
---|
| 73 | const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; |
---|
| 74 | const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; |
---|
| 75 | drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); |
---|
[1963] | 76 | } |
---|
| 77 | } |
---|
| 78 | } |
---|
| 79 | |
---|
| 80 | // |
---|
| 81 | template <typename T> |
---|
| 82 | static inline T sum(const btAlignedObjectArray<T>& items) |
---|
| 83 | { |
---|
[1972] | 84 | T v; |
---|
| 85 | if(items.size()) |
---|
[1963] | 86 | { |
---|
[1972] | 87 | v=items[0]; |
---|
| 88 | for(int i=1,ni=items.size();i<ni;++i) |
---|
[1963] | 89 | { |
---|
[1972] | 90 | v+=items[i]; |
---|
[1963] | 91 | } |
---|
| 92 | } |
---|
[1972] | 93 | return(v); |
---|
[1963] | 94 | } |
---|
| 95 | |
---|
| 96 | // |
---|
| 97 | template <typename T,typename Q> |
---|
| 98 | static inline void add(btAlignedObjectArray<T>& items,const Q& value) |
---|
| 99 | { |
---|
[1972] | 100 | for(int i=0,ni=items.size();i<ni;++i) |
---|
[1963] | 101 | { |
---|
[1972] | 102 | items[i]+=value; |
---|
[1963] | 103 | } |
---|
| 104 | } |
---|
| 105 | |
---|
| 106 | // |
---|
| 107 | template <typename T,typename Q> |
---|
| 108 | static inline void mul(btAlignedObjectArray<T>& items,const Q& value) |
---|
| 109 | { |
---|
[1972] | 110 | for(int i=0,ni=items.size();i<ni;++i) |
---|
[1963] | 111 | { |
---|
[1972] | 112 | items[i]*=value; |
---|
[1963] | 113 | } |
---|
| 114 | } |
---|
| 115 | |
---|
| 116 | // |
---|
| 117 | template <typename T> |
---|
| 118 | static inline T average(const btAlignedObjectArray<T>& items) |
---|
| 119 | { |
---|
[1972] | 120 | const btScalar n=(btScalar)(items.size()>0?items.size():1); |
---|
| 121 | return(sum(items)/n); |
---|
[1963] | 122 | } |
---|
| 123 | |
---|
| 124 | // |
---|
| 125 | static inline btScalar tetravolume(const btVector3& x0, |
---|
| 126 | const btVector3& x1, |
---|
| 127 | const btVector3& x2, |
---|
| 128 | const btVector3& x3) |
---|
| 129 | { |
---|
| 130 | const btVector3 a=x1-x0; |
---|
| 131 | const btVector3 b=x2-x0; |
---|
| 132 | const btVector3 c=x3-x0; |
---|
| 133 | return(dot(a,cross(b,c))); |
---|
| 134 | } |
---|
| 135 | |
---|
| 136 | // |
---|
| 137 | #if 0 |
---|
| 138 | static btVector3 stresscolor(btScalar stress) |
---|
[1972] | 139 | { |
---|
[1963] | 140 | static const btVector3 spectrum[]= { btVector3(1,0,1), |
---|
[1972] | 141 | btVector3(0,0,1), |
---|
| 142 | btVector3(0,1,1), |
---|
| 143 | btVector3(0,1,0), |
---|
| 144 | btVector3(1,1,0), |
---|
| 145 | btVector3(1,0,0), |
---|
| 146 | btVector3(1,0,0)}; |
---|
[1963] | 147 | static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; |
---|
| 148 | static const btScalar one=1; |
---|
| 149 | stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors; |
---|
| 150 | const int sel=(int)stress; |
---|
| 151 | const btScalar frc=stress-sel; |
---|
| 152 | return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); |
---|
[1972] | 153 | } |
---|
[1963] | 154 | #endif |
---|
| 155 | |
---|
| 156 | // |
---|
| 157 | void btSoftBodyHelpers::Draw( btSoftBody* psb, |
---|
[1972] | 158 | btIDebugDraw* idraw, |
---|
| 159 | int drawflags) |
---|
[1963] | 160 | { |
---|
| 161 | const btScalar scl=(btScalar)0.1; |
---|
| 162 | const btScalar nscl=scl*5; |
---|
| 163 | const btVector3 lcolor=btVector3(0,0,0); |
---|
| 164 | const btVector3 ncolor=btVector3(1,1,1); |
---|
| 165 | const btVector3 ccolor=btVector3(1,0,0); |
---|
| 166 | int i,j,nj; |
---|
| 167 | |
---|
| 168 | /* Nodes */ |
---|
| 169 | if(0!=(drawflags&fDrawFlags::Nodes)) |
---|
| 170 | { |
---|
| 171 | for(i=0;i<psb->m_nodes.size();++i) |
---|
| 172 | { |
---|
| 173 | const btSoftBody::Node& n=psb->m_nodes[i]; |
---|
| 174 | if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
---|
| 175 | idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); |
---|
| 176 | idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); |
---|
| 177 | idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); |
---|
| 178 | } |
---|
| 179 | } |
---|
| 180 | /* Links */ |
---|
| 181 | if(0!=(drawflags&fDrawFlags::Links)) |
---|
| 182 | { |
---|
| 183 | for(i=0;i<psb->m_links.size();++i) |
---|
| 184 | { |
---|
| 185 | const btSoftBody::Link& l=psb->m_links[i]; |
---|
| 186 | if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
---|
| 187 | idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); |
---|
| 188 | } |
---|
| 189 | } |
---|
| 190 | /* Normals */ |
---|
| 191 | if(0!=(drawflags&fDrawFlags::Normals)) |
---|
| 192 | { |
---|
| 193 | for(i=0;i<psb->m_nodes.size();++i) |
---|
| 194 | { |
---|
| 195 | const btSoftBody::Node& n=psb->m_nodes[i]; |
---|
| 196 | if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
---|
| 197 | const btVector3 d=n.m_n*nscl; |
---|
| 198 | idraw->drawLine(n.m_x,n.m_x+d,ncolor); |
---|
| 199 | idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); |
---|
| 200 | } |
---|
| 201 | } |
---|
| 202 | /* Contacts */ |
---|
| 203 | if(0!=(drawflags&fDrawFlags::Contacts)) |
---|
| 204 | { |
---|
| 205 | static const btVector3 axis[]={btVector3(1,0,0), |
---|
| 206 | btVector3(0,1,0), |
---|
| 207 | btVector3(0,0,1)}; |
---|
| 208 | for(i=0;i<psb->m_rcontacts.size();++i) |
---|
| 209 | { |
---|
| 210 | const btSoftBody::RContact& c=psb->m_rcontacts[i]; |
---|
| 211 | const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* |
---|
| 212 | (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); |
---|
| 213 | const btVector3 x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); |
---|
| 214 | const btVector3 y=cross(x,c.m_cti.m_normal).normalized(); |
---|
| 215 | idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); |
---|
| 216 | idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); |
---|
| 217 | idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); |
---|
| 218 | } |
---|
| 219 | } |
---|
| 220 | /* Anchors */ |
---|
| 221 | if(0!=(drawflags&fDrawFlags::Anchors)) |
---|
| 222 | { |
---|
| 223 | for(i=0;i<psb->m_anchors.size();++i) |
---|
| 224 | { |
---|
| 225 | const btSoftBody::Anchor& a=psb->m_anchors[i]; |
---|
| 226 | const btVector3 q=a.m_body->getWorldTransform()*a.m_local; |
---|
| 227 | drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); |
---|
| 228 | drawVertex(idraw,q,0.25,btVector3(0,1,0)); |
---|
| 229 | idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); |
---|
| 230 | } |
---|
| 231 | for(i=0;i<psb->m_nodes.size();++i) |
---|
| 232 | { |
---|
| 233 | const btSoftBody::Node& n=psb->m_nodes[i]; |
---|
| 234 | if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
---|
| 235 | if(n.m_im<=0) |
---|
| 236 | { |
---|
| 237 | drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); |
---|
| 238 | } |
---|
| 239 | } |
---|
| 240 | } |
---|
| 241 | /* Faces */ |
---|
| 242 | if(0!=(drawflags&fDrawFlags::Faces)) |
---|
| 243 | { |
---|
| 244 | const btScalar scl=(btScalar)0.8; |
---|
| 245 | const btScalar alp=(btScalar)1; |
---|
| 246 | const btVector3 col(0,(btScalar)0.7,0); |
---|
| 247 | for(i=0;i<psb->m_faces.size();++i) |
---|
| 248 | { |
---|
| 249 | const btSoftBody::Face& f=psb->m_faces[i]; |
---|
| 250 | if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; |
---|
| 251 | const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; |
---|
| 252 | const btVector3 c=(x[0]+x[1]+x[2])/3; |
---|
| 253 | idraw->drawTriangle((x[0]-c)*scl+c, |
---|
[1972] | 254 | (x[1]-c)*scl+c, |
---|
| 255 | (x[2]-c)*scl+c, |
---|
| 256 | col,alp); |
---|
[1963] | 257 | } |
---|
| 258 | } |
---|
| 259 | /* Clusters */ |
---|
| 260 | if(0!=(drawflags&fDrawFlags::Clusters)) |
---|
[1972] | 261 | { |
---|
[1963] | 262 | srand(1806); |
---|
| 263 | for(i=0;i<psb->m_clusters.size();++i) |
---|
[1972] | 264 | { |
---|
[1963] | 265 | if(psb->m_clusters[i]->m_collide) |
---|
[1972] | 266 | { |
---|
[1963] | 267 | btVector3 color( rand()/(btScalar)RAND_MAX, |
---|
[1972] | 268 | rand()/(btScalar)RAND_MAX, |
---|
| 269 | rand()/(btScalar)RAND_MAX); |
---|
[1963] | 270 | color=color.normalized()*0.75; |
---|
| 271 | btAlignedObjectArray<btVector3> vertices; |
---|
| 272 | vertices.resize(psb->m_clusters[i]->m_nodes.size()); |
---|
| 273 | for(j=0,nj=vertices.size();j<nj;++j) |
---|
[1972] | 274 | { |
---|
[1963] | 275 | vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; |
---|
[1972] | 276 | } |
---|
[1963] | 277 | HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); |
---|
| 278 | HullResult hres; |
---|
| 279 | HullLibrary hlib; |
---|
| 280 | hdsc.mMaxVertices=vertices.size(); |
---|
| 281 | hlib.CreateConvexHull(hdsc,hres); |
---|
| 282 | const btVector3 center=average(hres.m_OutputVertices); |
---|
| 283 | add(hres.m_OutputVertices,-center); |
---|
| 284 | mul(hres.m_OutputVertices,(btScalar)1); |
---|
| 285 | add(hres.m_OutputVertices,center); |
---|
| 286 | for(j=0;j<(int)hres.mNumFaces;++j) |
---|
[1972] | 287 | { |
---|
[1963] | 288 | const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; |
---|
| 289 | idraw->drawTriangle(hres.m_OutputVertices[idx[0]], |
---|
[1972] | 290 | hres.m_OutputVertices[idx[1]], |
---|
| 291 | hres.m_OutputVertices[idx[2]], |
---|
| 292 | color,1); |
---|
| 293 | } |
---|
| 294 | hlib.ReleaseResult(hres); |
---|
[1963] | 295 | } |
---|
| 296 | /* Velocities */ |
---|
[1972] | 297 | #if 0 |
---|
[1963] | 298 | for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) |
---|
[1972] | 299 | { |
---|
[1963] | 300 | const btSoftBody::Cluster& c=psb->m_clusters[i]; |
---|
| 301 | const btVector3 r=c.m_nodes[j]->m_x-c.m_com; |
---|
| 302 | const btVector3 v=c.m_lv+cross(c.m_av,r); |
---|
| 303 | idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); |
---|
[1972] | 304 | } |
---|
| 305 | #endif |
---|
[1963] | 306 | /* Frame */ |
---|
| 307 | btSoftBody::Cluster& c=*psb->m_clusters[i]; |
---|
| 308 | idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); |
---|
| 309 | idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); |
---|
| 310 | idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); |
---|
[1972] | 311 | } |
---|
[1963] | 312 | } |
---|
| 313 | /* Notes */ |
---|
| 314 | if(0!=(drawflags&fDrawFlags::Notes)) |
---|
| 315 | { |
---|
| 316 | for(i=0;i<psb->m_notes.size();++i) |
---|
| 317 | { |
---|
| 318 | const btSoftBody::Note& n=psb->m_notes[i]; |
---|
| 319 | btVector3 p=n.m_offset; |
---|
| 320 | for(int j=0;j<n.m_rank;++j) |
---|
[1972] | 321 | { |
---|
[1963] | 322 | p+=n.m_nodes[j]->m_x*n.m_coords[j]; |
---|
[1972] | 323 | } |
---|
[1963] | 324 | idraw->draw3dText(p,n.m_text); |
---|
| 325 | } |
---|
| 326 | } |
---|
| 327 | /* Node tree */ |
---|
| 328 | if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); |
---|
| 329 | /* Face tree */ |
---|
| 330 | if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); |
---|
| 331 | /* Cluster tree */ |
---|
| 332 | if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); |
---|
| 333 | /* Joints */ |
---|
| 334 | if(0!=(drawflags&fDrawFlags::Joints)) |
---|
| 335 | { |
---|
[1972] | 336 | for(i=0;i<psb->m_joints.size();++i) |
---|
[1963] | 337 | { |
---|
[1972] | 338 | const btSoftBody::Joint* pj=psb->m_joints[i]; |
---|
| 339 | switch(pj->Type()) |
---|
[1963] | 340 | { |
---|
| 341 | case btSoftBody::Joint::eType::Linear: |
---|
| 342 | { |
---|
[1972] | 343 | const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; |
---|
| 344 | const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; |
---|
| 345 | const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; |
---|
| 346 | idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); |
---|
| 347 | idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); |
---|
| 348 | drawVertex(idraw,a0,0.25,btVector3(1,1,0)); |
---|
| 349 | drawVertex(idraw,a1,0.25,btVector3(0,1,1)); |
---|
[1963] | 350 | } |
---|
[1972] | 351 | break; |
---|
[1963] | 352 | case btSoftBody::Joint::eType::Angular: |
---|
| 353 | { |
---|
[1972] | 354 | const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; |
---|
| 355 | const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); |
---|
| 356 | const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); |
---|
| 357 | const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; |
---|
| 358 | const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; |
---|
| 359 | idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); |
---|
| 360 | idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); |
---|
| 361 | idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); |
---|
| 362 | idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); |
---|
[1963] | 363 | } |
---|
| 364 | } |
---|
| 365 | } |
---|
| 366 | } |
---|
| 367 | } |
---|
| 368 | |
---|
| 369 | // |
---|
| 370 | void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, |
---|
[1972] | 371 | btIDebugDraw* idraw, |
---|
| 372 | bool masses, |
---|
| 373 | bool areas, |
---|
| 374 | bool /*stress*/) |
---|
[1963] | 375 | { |
---|
| 376 | for(int i=0;i<psb->m_nodes.size();++i) |
---|
| 377 | { |
---|
| 378 | const btSoftBody::Node& n=psb->m_nodes[i]; |
---|
| 379 | char text[2048]={0}; |
---|
| 380 | char buff[1024]; |
---|
| 381 | if(masses) |
---|
| 382 | { |
---|
| 383 | sprintf(buff," M(%.2f)",1/n.m_im); |
---|
| 384 | strcat(text,buff); |
---|
| 385 | } |
---|
| 386 | if(areas) |
---|
| 387 | { |
---|
| 388 | sprintf(buff," A(%.2f)",n.m_area); |
---|
| 389 | strcat(text,buff); |
---|
| 390 | } |
---|
| 391 | if(text[0]) idraw->draw3dText(n.m_x,text); |
---|
| 392 | } |
---|
| 393 | } |
---|
| 394 | |
---|
| 395 | // |
---|
| 396 | void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, |
---|
[1972] | 397 | btIDebugDraw* idraw, |
---|
| 398 | int mindepth, |
---|
| 399 | int maxdepth) |
---|
[1963] | 400 | { |
---|
[1972] | 401 | drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); |
---|
[1963] | 402 | } |
---|
| 403 | |
---|
| 404 | // |
---|
| 405 | void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, |
---|
[1972] | 406 | btIDebugDraw* idraw, |
---|
| 407 | int mindepth, |
---|
| 408 | int maxdepth) |
---|
[1963] | 409 | { |
---|
[1972] | 410 | drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); |
---|
[1963] | 411 | } |
---|
| 412 | |
---|
| 413 | // |
---|
| 414 | void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, |
---|
[1972] | 415 | btIDebugDraw* idraw, |
---|
| 416 | int mindepth, |
---|
| 417 | int maxdepth) |
---|
[1963] | 418 | { |
---|
[1972] | 419 | drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); |
---|
[1963] | 420 | } |
---|
| 421 | |
---|
| 422 | // |
---|
| 423 | void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, |
---|
[1972] | 424 | btIDebugDraw* idraw) |
---|
[1963] | 425 | { |
---|
| 426 | if(psb->m_pose.m_bframe) |
---|
| 427 | { |
---|
| 428 | static const btScalar ascl=10; |
---|
| 429 | static const btScalar nscl=(btScalar)0.1; |
---|
| 430 | const btVector3 com=psb->m_pose.m_com; |
---|
| 431 | const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; |
---|
| 432 | const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); |
---|
| 433 | const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); |
---|
| 434 | const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); |
---|
| 435 | idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); |
---|
| 436 | idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); |
---|
| 437 | idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); |
---|
| 438 | for(int i=0;i<psb->m_pose.m_pos.size();++i) |
---|
| 439 | { |
---|
| 440 | const btVector3 x=com+trs*psb->m_pose.m_pos[i]; |
---|
| 441 | drawVertex(idraw,x,nscl,btVector3(1,0,1)); |
---|
| 442 | } |
---|
| 443 | } |
---|
| 444 | } |
---|
| 445 | |
---|
| 446 | // |
---|
| 447 | btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, |
---|
[1972] | 448 | const btVector3& to, |
---|
| 449 | int res, |
---|
| 450 | int fixeds) |
---|
[1963] | 451 | { |
---|
| 452 | /* Create nodes */ |
---|
| 453 | const int r=res+2; |
---|
| 454 | btVector3* x=new btVector3[r]; |
---|
| 455 | btScalar* m=new btScalar[r]; |
---|
| 456 | int i; |
---|
| 457 | |
---|
| 458 | for(i=0;i<r;++i) |
---|
| 459 | { |
---|
| 460 | const btScalar t=i/(btScalar)(r-1); |
---|
| 461 | x[i]=lerp(from,to,t); |
---|
| 462 | m[i]=1; |
---|
| 463 | } |
---|
| 464 | btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m); |
---|
| 465 | if(fixeds&1) psb->setMass(0,0); |
---|
| 466 | if(fixeds&2) psb->setMass(r-1,0); |
---|
| 467 | delete[] x; |
---|
| 468 | delete[] m; |
---|
| 469 | /* Create links */ |
---|
| 470 | for(i=1;i<r;++i) |
---|
| 471 | { |
---|
| 472 | psb->appendLink(i-1,i); |
---|
| 473 | } |
---|
| 474 | /* Finished */ |
---|
| 475 | return(psb); |
---|
| 476 | } |
---|
| 477 | |
---|
| 478 | // |
---|
| 479 | btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, |
---|
[1972] | 480 | const btVector3& corner10, |
---|
| 481 | const btVector3& corner01, |
---|
| 482 | const btVector3& corner11, |
---|
| 483 | int resx, |
---|
| 484 | int resy, |
---|
| 485 | int fixeds, |
---|
| 486 | bool gendiags) |
---|
[1963] | 487 | { |
---|
| 488 | #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) |
---|
| 489 | /* Create nodes */ |
---|
| 490 | if((resx<2)||(resy<2)) return(0); |
---|
| 491 | const int rx=resx; |
---|
| 492 | const int ry=resy; |
---|
| 493 | const int tot=rx*ry; |
---|
| 494 | btVector3* x=new btVector3[tot]; |
---|
| 495 | btScalar* m=new btScalar[tot]; |
---|
| 496 | int iy; |
---|
| 497 | |
---|
| 498 | for(iy=0;iy<ry;++iy) |
---|
| 499 | { |
---|
| 500 | const btScalar ty=iy/(btScalar)(ry-1); |
---|
| 501 | const btVector3 py0=lerp(corner00,corner01,ty); |
---|
| 502 | const btVector3 py1=lerp(corner10,corner11,ty); |
---|
| 503 | for(int ix=0;ix<rx;++ix) |
---|
| 504 | { |
---|
| 505 | const btScalar tx=ix/(btScalar)(rx-1); |
---|
| 506 | x[IDX(ix,iy)]=lerp(py0,py1,tx); |
---|
| 507 | m[IDX(ix,iy)]=1; |
---|
| 508 | } |
---|
| 509 | } |
---|
| 510 | btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); |
---|
| 511 | if(fixeds&1) psb->setMass(IDX(0,0),0); |
---|
| 512 | if(fixeds&2) psb->setMass(IDX(rx-1,0),0); |
---|
| 513 | if(fixeds&4) psb->setMass(IDX(0,ry-1),0); |
---|
| 514 | if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); |
---|
| 515 | delete[] x; |
---|
| 516 | delete[] m; |
---|
| 517 | /* Create links and faces */ |
---|
| 518 | for(iy=0;iy<ry;++iy) |
---|
| 519 | { |
---|
| 520 | for(int ix=0;ix<rx;++ix) |
---|
| 521 | { |
---|
| 522 | const int idx=IDX(ix,iy); |
---|
| 523 | const bool mdx=(ix+1)<rx; |
---|
| 524 | const bool mdy=(iy+1)<ry; |
---|
| 525 | if(mdx) psb->appendLink(idx,IDX(ix+1,iy)); |
---|
| 526 | if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); |
---|
| 527 | if(mdx&&mdy) |
---|
| 528 | { |
---|
| 529 | if((ix+iy)&1) |
---|
| 530 | { |
---|
| 531 | psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); |
---|
| 532 | psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); |
---|
| 533 | if(gendiags) |
---|
| 534 | { |
---|
| 535 | psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); |
---|
| 536 | } |
---|
| 537 | } |
---|
| 538 | else |
---|
| 539 | { |
---|
| 540 | psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); |
---|
| 541 | psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); |
---|
| 542 | if(gendiags) |
---|
| 543 | { |
---|
| 544 | psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); |
---|
| 545 | } |
---|
| 546 | } |
---|
| 547 | } |
---|
| 548 | } |
---|
| 549 | } |
---|
| 550 | /* Finished */ |
---|
| 551 | #undef IDX |
---|
| 552 | return(psb); |
---|
| 553 | } |
---|
| 554 | |
---|
| 555 | // |
---|
| 556 | btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, |
---|
[1972] | 557 | const btVector3& radius, |
---|
| 558 | int res) |
---|
[1963] | 559 | { |
---|
| 560 | struct Hammersley |
---|
| 561 | { |
---|
| 562 | static void Generate(btVector3* x,int n) |
---|
| 563 | { |
---|
| 564 | for(int i=0;i<n;i++) |
---|
| 565 | { |
---|
| 566 | btScalar p=0.5,t=0; |
---|
| 567 | for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p; |
---|
| 568 | btScalar w=2*t-1; |
---|
| 569 | btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; |
---|
| 570 | btScalar s=btSqrt(1-w*w); |
---|
| 571 | *x++=btVector3(s*btCos(a),s*btSin(a),w); |
---|
| 572 | } |
---|
| 573 | } |
---|
| 574 | }; |
---|
| 575 | btAlignedObjectArray<btVector3> vtx; |
---|
| 576 | vtx.resize(3+res); |
---|
| 577 | Hammersley::Generate(&vtx[0],vtx.size()); |
---|
| 578 | for(int i=0;i<vtx.size();++i) |
---|
| 579 | { |
---|
| 580 | vtx[i]=vtx[i]*radius+center; |
---|
| 581 | } |
---|
| 582 | return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); |
---|
| 583 | } |
---|
| 584 | |
---|
| 585 | |
---|
| 586 | |
---|
| 587 | // |
---|
| 588 | btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, |
---|
[1972] | 589 | const int* triangles, |
---|
| 590 | int ntriangles) |
---|
[1963] | 591 | { |
---|
| 592 | int maxidx=0; |
---|
| 593 | int i,j,ni; |
---|
| 594 | |
---|
| 595 | for(i=0,ni=ntriangles*3;i<ni;++i) |
---|
| 596 | { |
---|
| 597 | maxidx=btMax(triangles[i],maxidx); |
---|
| 598 | } |
---|
| 599 | ++maxidx; |
---|
| 600 | btAlignedObjectArray<bool> chks; |
---|
| 601 | btAlignedObjectArray<btVector3> vtx; |
---|
| 602 | chks.resize(maxidx*maxidx,false); |
---|
| 603 | vtx.resize(maxidx); |
---|
| 604 | for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3) |
---|
| 605 | { |
---|
| 606 | vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); |
---|
| 607 | } |
---|
| 608 | btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0); |
---|
| 609 | for( i=0,ni=ntriangles*3;i<ni;i+=3) |
---|
| 610 | { |
---|
| 611 | const int idx[]={triangles[i],triangles[i+1],triangles[i+2]}; |
---|
| 612 | #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) |
---|
| 613 | for(int j=2,k=0;k<3;j=k++) |
---|
| 614 | { |
---|
| 615 | if(!chks[IDX(idx[j],idx[k])]) |
---|
| 616 | { |
---|
| 617 | chks[IDX(idx[j],idx[k])]=true; |
---|
| 618 | chks[IDX(idx[k],idx[k])]=true; |
---|
| 619 | psb->appendLink(idx[j],idx[k]); |
---|
| 620 | } |
---|
| 621 | } |
---|
| 622 | #undef IDX |
---|
| 623 | psb->appendFace(idx[0],idx[1],idx[2]); |
---|
| 624 | } |
---|
| 625 | psb->randomizeConstraints(); |
---|
| 626 | return(psb); |
---|
| 627 | } |
---|
| 628 | |
---|
| 629 | // |
---|
| 630 | btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, |
---|
[1972] | 631 | int nvertices) |
---|
[1963] | 632 | { |
---|
| 633 | HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); |
---|
| 634 | HullResult hres; |
---|
| 635 | HullLibrary hlib;/*??*/ |
---|
| 636 | hdsc.mMaxVertices=nvertices; |
---|
| 637 | hlib.CreateConvexHull(hdsc,hres); |
---|
| 638 | btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, |
---|
| 639 | &hres.m_OutputVertices[0],0); |
---|
| 640 | for(int i=0;i<(int)hres.mNumFaces;++i) |
---|
| 641 | { |
---|
| 642 | const int idx[]={ hres.m_Indices[i*3+0], |
---|
| 643 | hres.m_Indices[i*3+1], |
---|
| 644 | hres.m_Indices[i*3+2]}; |
---|
| 645 | if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); |
---|
| 646 | if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); |
---|
| 647 | if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); |
---|
| 648 | psb->appendFace(idx[0],idx[1],idx[2]); |
---|
| 649 | } |
---|
| 650 | hlib.ReleaseResult(hres); |
---|
| 651 | psb->randomizeConstraints(); |
---|
| 652 | return(psb); |
---|
| 653 | } |
---|