[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, |
---|
| 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 | } |
---|
| 31 | |
---|
| 32 | // |
---|
| 33 | static void drawBox( btIDebugDraw* idraw, |
---|
| 34 | const btVector3& mins, |
---|
| 35 | const btVector3& maxs, |
---|
| 36 | const btVector3& color) |
---|
| 37 | { |
---|
| 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); |
---|
| 52 | } |
---|
| 53 | |
---|
| 54 | // |
---|
| 55 | static void drawTree( btIDebugDraw* idraw, |
---|
| 56 | const btDbvtNode* node, |
---|
| 57 | int depth, |
---|
| 58 | const btVector3& ncolor, |
---|
| 59 | const btVector3& lcolor, |
---|
| 60 | int mindepth, |
---|
| 61 | int maxdepth) |
---|
| 62 | { |
---|
| 63 | if(node) |
---|
| 64 | { |
---|
| 65 | if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0))) |
---|
| 66 | { |
---|
| 67 | drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); |
---|
| 68 | drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); |
---|
| 69 | } |
---|
| 70 | if(depth>=mindepth) |
---|
| 71 | { |
---|
| 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); |
---|
| 76 | } |
---|
| 77 | } |
---|
| 78 | } |
---|
| 79 | |
---|
| 80 | // |
---|
| 81 | template <typename T> |
---|
| 82 | static inline T sum(const btAlignedObjectArray<T>& items) |
---|
| 83 | { |
---|
| 84 | T v; |
---|
| 85 | if(items.size()) |
---|
| 86 | { |
---|
| 87 | v=items[0]; |
---|
| 88 | for(int i=1,ni=items.size();i<ni;++i) |
---|
| 89 | { |
---|
| 90 | v+=items[i]; |
---|
| 91 | } |
---|
| 92 | } |
---|
| 93 | return(v); |
---|
| 94 | } |
---|
| 95 | |
---|
| 96 | // |
---|
| 97 | template <typename T,typename Q> |
---|
| 98 | static inline void add(btAlignedObjectArray<T>& items,const Q& value) |
---|
| 99 | { |
---|
| 100 | for(int i=0,ni=items.size();i<ni;++i) |
---|
| 101 | { |
---|
| 102 | items[i]+=value; |
---|
| 103 | } |
---|
| 104 | } |
---|
| 105 | |
---|
| 106 | // |
---|
| 107 | template <typename T,typename Q> |
---|
| 108 | static inline void mul(btAlignedObjectArray<T>& items,const Q& value) |
---|
| 109 | { |
---|
| 110 | for(int i=0,ni=items.size();i<ni;++i) |
---|
| 111 | { |
---|
| 112 | items[i]*=value; |
---|
| 113 | } |
---|
| 114 | } |
---|
| 115 | |
---|
| 116 | // |
---|
| 117 | template <typename T> |
---|
| 118 | static inline T average(const btAlignedObjectArray<T>& items) |
---|
| 119 | { |
---|
| 120 | const btScalar n=(btScalar)(items.size()>0?items.size():1); |
---|
| 121 | return(sum(items)/n); |
---|
| 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) |
---|
| 139 | { |
---|
| 140 | static const btVector3 spectrum[]= { btVector3(1,0,1), |
---|
| 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)}; |
---|
| 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); |
---|
| 153 | } |
---|
| 154 | #endif |
---|
| 155 | |
---|
| 156 | // |
---|
| 157 | void btSoftBodyHelpers::Draw( btSoftBody* psb, |
---|
| 158 | btIDebugDraw* idraw, |
---|
| 159 | int drawflags) |
---|
| 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, |
---|
| 254 | (x[1]-c)*scl+c, |
---|
| 255 | (x[2]-c)*scl+c, |
---|
| 256 | col,alp); |
---|
| 257 | } |
---|
| 258 | } |
---|
| 259 | /* Clusters */ |
---|
| 260 | if(0!=(drawflags&fDrawFlags::Clusters)) |
---|
| 261 | { |
---|
| 262 | srand(1806); |
---|
| 263 | for(i=0;i<psb->m_clusters.size();++i) |
---|
| 264 | { |
---|
| 265 | if(psb->m_clusters[i]->m_collide) |
---|
| 266 | { |
---|
| 267 | btVector3 color( rand()/(btScalar)RAND_MAX, |
---|
| 268 | rand()/(btScalar)RAND_MAX, |
---|
| 269 | rand()/(btScalar)RAND_MAX); |
---|
| 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) |
---|
| 274 | { |
---|
| 275 | vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; |
---|
| 276 | } |
---|
| 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) |
---|
| 287 | { |
---|
| 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]], |
---|
| 290 | hres.m_OutputVertices[idx[1]], |
---|
| 291 | hres.m_OutputVertices[idx[2]], |
---|
| 292 | color,1); |
---|
| 293 | } |
---|
| 294 | hlib.ReleaseResult(hres); |
---|
| 295 | } |
---|
| 296 | /* Velocities */ |
---|
| 297 | #if 0 |
---|
| 298 | for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j) |
---|
| 299 | { |
---|
| 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)); |
---|
| 304 | } |
---|
| 305 | #endif |
---|
| 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)); |
---|
| 311 | } |
---|
| 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) |
---|
| 321 | { |
---|
| 322 | p+=n.m_nodes[j]->m_x*n.m_coords[j]; |
---|
| 323 | } |
---|
| 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 | { |
---|
| 336 | for(i=0;i<psb->m_joints.size();++i) |
---|
| 337 | { |
---|
| 338 | const btSoftBody::Joint* pj=psb->m_joints[i]; |
---|
| 339 | switch(pj->Type()) |
---|
| 340 | { |
---|
| 341 | case btSoftBody::Joint::eType::Linear: |
---|
| 342 | { |
---|
| 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)); |
---|
| 350 | } |
---|
| 351 | break; |
---|
| 352 | case btSoftBody::Joint::eType::Angular: |
---|
| 353 | { |
---|
| 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)); |
---|
| 363 | } |
---|
| 364 | } |
---|
| 365 | } |
---|
| 366 | } |
---|
| 367 | } |
---|
| 368 | |
---|
| 369 | // |
---|
| 370 | void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, |
---|
| 371 | btIDebugDraw* idraw, |
---|
| 372 | bool masses, |
---|
| 373 | bool areas, |
---|
| 374 | bool /*stress*/) |
---|
| 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, |
---|
| 397 | btIDebugDraw* idraw, |
---|
| 398 | int mindepth, |
---|
| 399 | int maxdepth) |
---|
| 400 | { |
---|
| 401 | drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); |
---|
| 402 | } |
---|
| 403 | |
---|
| 404 | // |
---|
| 405 | void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, |
---|
| 406 | btIDebugDraw* idraw, |
---|
| 407 | int mindepth, |
---|
| 408 | int maxdepth) |
---|
| 409 | { |
---|
| 410 | drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); |
---|
| 411 | } |
---|
| 412 | |
---|
| 413 | // |
---|
| 414 | void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, |
---|
| 415 | btIDebugDraw* idraw, |
---|
| 416 | int mindepth, |
---|
| 417 | int maxdepth) |
---|
| 418 | { |
---|
| 419 | drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); |
---|
| 420 | } |
---|
| 421 | |
---|
| 422 | // |
---|
| 423 | void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, |
---|
| 424 | btIDebugDraw* idraw) |
---|
| 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, |
---|
| 448 | const btVector3& to, |
---|
| 449 | int res, |
---|
| 450 | int fixeds) |
---|
| 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, |
---|
| 480 | const btVector3& corner10, |
---|
| 481 | const btVector3& corner01, |
---|
| 482 | const btVector3& corner11, |
---|
| 483 | int resx, |
---|
| 484 | int resy, |
---|
| 485 | int fixeds, |
---|
| 486 | bool gendiags) |
---|
| 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::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, |
---|
| 557 | const btVector3& corner00, |
---|
| 558 | const btVector3& corner10, |
---|
| 559 | const btVector3& corner01, |
---|
| 560 | const btVector3& corner11, |
---|
| 561 | int resx, |
---|
| 562 | int resy, |
---|
| 563 | int fixeds, |
---|
| 564 | bool gendiags, |
---|
| 565 | float* tex_coords) |
---|
| 566 | { |
---|
| 567 | |
---|
| 568 | /* |
---|
| 569 | * |
---|
| 570 | * corners: |
---|
| 571 | * |
---|
| 572 | * [0][0] corner00 ------- corner01 [resx][0] |
---|
| 573 | * | | |
---|
| 574 | * | | |
---|
| 575 | * [0][resy] corner10 -------- corner11 [resx][resy] |
---|
| 576 | * |
---|
| 577 | * |
---|
| 578 | * |
---|
| 579 | * |
---|
| 580 | * |
---|
| 581 | * |
---|
| 582 | * "fixedgs" map: |
---|
| 583 | * |
---|
| 584 | * corner00 --> +1 |
---|
| 585 | * corner01 --> +2 |
---|
| 586 | * corner10 --> +4 |
---|
| 587 | * corner11 --> +8 |
---|
| 588 | * upper middle --> +16 |
---|
| 589 | * left middle --> +32 |
---|
| 590 | * right middle --> +64 |
---|
| 591 | * lower middle --> +128 |
---|
| 592 | * center --> +256 |
---|
| 593 | * |
---|
| 594 | * |
---|
| 595 | * tex_coords size (resx-1)*(resy-1)*12 |
---|
| 596 | * |
---|
| 597 | * |
---|
| 598 | * |
---|
| 599 | * SINGLE QUAD INTERNALS |
---|
| 600 | * |
---|
| 601 | * 1) btSoftBody's nodes and links, |
---|
| 602 | * diagonal link is optional ("gendiags") |
---|
| 603 | * |
---|
| 604 | * |
---|
| 605 | * node00 ------ node01 |
---|
| 606 | * | . |
---|
| 607 | * | . |
---|
| 608 | * | . |
---|
| 609 | * | . |
---|
| 610 | * | . |
---|
| 611 | * node10 node11 |
---|
| 612 | * |
---|
| 613 | * |
---|
| 614 | * |
---|
| 615 | * 2) Faces: |
---|
| 616 | * two triangles, |
---|
| 617 | * UV Coordinates (hier example for single quad) |
---|
| 618 | * |
---|
| 619 | * (0,1) (0,1) (1,1) |
---|
| 620 | * 1 |\ 3 \-----| 2 |
---|
| 621 | * | \ \ | |
---|
| 622 | * | \ \ | |
---|
| 623 | * | \ \ | |
---|
| 624 | * | \ \ | |
---|
| 625 | * 2 |-----\ 3 \| 1 |
---|
| 626 | * (0,0) (1,0) (1,0) |
---|
| 627 | * |
---|
| 628 | * |
---|
| 629 | * |
---|
| 630 | * |
---|
| 631 | * |
---|
| 632 | * |
---|
| 633 | */ |
---|
| 634 | |
---|
| 635 | #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) |
---|
| 636 | /* Create nodes */ |
---|
| 637 | if((resx<2)||(resy<2)) return(0); |
---|
| 638 | const int rx=resx; |
---|
| 639 | const int ry=resy; |
---|
| 640 | const int tot=rx*ry; |
---|
| 641 | btVector3* x=new btVector3[tot]; |
---|
| 642 | btScalar* m=new btScalar[tot]; |
---|
| 643 | |
---|
| 644 | for(int iy=0;iy<ry;++iy) |
---|
| 645 | { |
---|
| 646 | const btScalar ty=iy/(btScalar)(ry-1); |
---|
| 647 | const btVector3 py0=lerp(corner00,corner01,ty); |
---|
| 648 | const btVector3 py1=lerp(corner10,corner11,ty); |
---|
| 649 | for(int ix=0;ix<rx;++ix) |
---|
| 650 | { |
---|
| 651 | const btScalar tx=ix/(btScalar)(rx-1); |
---|
| 652 | x[IDX(ix,iy)]=lerp(py0,py1,tx); |
---|
| 653 | m[IDX(ix,iy)]=1; |
---|
| 654 | } |
---|
| 655 | } |
---|
| 656 | btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m); |
---|
| 657 | if(fixeds&1) psb->setMass(IDX(0,0),0); |
---|
| 658 | if(fixeds&2) psb->setMass(IDX(rx-1,0),0); |
---|
| 659 | if(fixeds&4) psb->setMass(IDX(0,ry-1),0); |
---|
| 660 | if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); |
---|
| 661 | if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); |
---|
| 662 | if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); |
---|
| 663 | if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); |
---|
| 664 | if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); |
---|
| 665 | if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); |
---|
| 666 | delete[] x; |
---|
| 667 | delete[] m; |
---|
| 668 | |
---|
| 669 | |
---|
| 670 | int z = 0; |
---|
| 671 | /* Create links and faces */ |
---|
| 672 | for(int iy=0;iy<ry;++iy) |
---|
| 673 | { |
---|
| 674 | for(int ix=0;ix<rx;++ix) |
---|
| 675 | { |
---|
| 676 | const bool mdx=(ix+1)<rx; |
---|
| 677 | const bool mdy=(iy+1)<ry; |
---|
| 678 | |
---|
| 679 | int node00=IDX(ix,iy); |
---|
| 680 | int node01=IDX(ix+1,iy); |
---|
| 681 | int node10=IDX(ix,iy+1); |
---|
| 682 | int node11=IDX(ix+1,iy+1); |
---|
| 683 | |
---|
| 684 | if(mdx) psb->appendLink(node00,node01); |
---|
| 685 | if(mdy) psb->appendLink(node00,node10); |
---|
| 686 | if(mdx&&mdy) |
---|
| 687 | { |
---|
| 688 | psb->appendFace(node00,node10,node11); |
---|
| 689 | if (tex_coords) { |
---|
| 690 | tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); |
---|
| 691 | tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); |
---|
| 692 | tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); |
---|
| 693 | tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); |
---|
| 694 | tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); |
---|
| 695 | tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); |
---|
| 696 | } |
---|
| 697 | psb->appendFace(node11,node01,node00); |
---|
| 698 | if (tex_coords) { |
---|
| 699 | tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); |
---|
| 700 | tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); |
---|
| 701 | tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); |
---|
| 702 | tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); |
---|
| 703 | tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); |
---|
| 704 | tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); |
---|
| 705 | } |
---|
| 706 | if (gendiags) psb->appendLink(node00,node11); |
---|
| 707 | z += 12; |
---|
| 708 | } |
---|
| 709 | } |
---|
| 710 | } |
---|
| 711 | /* Finished */ |
---|
| 712 | #undef IDX |
---|
| 713 | return(psb); |
---|
| 714 | } |
---|
| 715 | |
---|
| 716 | float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) |
---|
| 717 | { |
---|
| 718 | |
---|
| 719 | /* |
---|
| 720 | * |
---|
| 721 | * |
---|
| 722 | * node00 --- node01 |
---|
| 723 | * | | |
---|
| 724 | * node10 --- node11 |
---|
| 725 | * |
---|
| 726 | * |
---|
| 727 | * ID map: |
---|
| 728 | * |
---|
| 729 | * node00 s --> 0 |
---|
| 730 | * node00 t --> 1 |
---|
| 731 | * |
---|
| 732 | * node01 s --> 3 |
---|
| 733 | * node01 t --> 1 |
---|
| 734 | * |
---|
| 735 | * node10 s --> 0 |
---|
| 736 | * node10 t --> 2 |
---|
| 737 | * |
---|
| 738 | * node11 s --> 3 |
---|
| 739 | * node11 t --> 2 |
---|
| 740 | * |
---|
| 741 | * |
---|
| 742 | */ |
---|
| 743 | |
---|
| 744 | float tc=0.0f; |
---|
| 745 | if (id == 0) { |
---|
| 746 | tc = (1.0f/((resx-1))*ix); |
---|
| 747 | } |
---|
| 748 | else if (id==1) { |
---|
| 749 | tc = (1.0f/((resy-1))*(resy-1-iy)); |
---|
| 750 | } |
---|
| 751 | else if (id==2) { |
---|
| 752 | tc = (1.0f/((resy-1))*(resy-1-iy-1)); |
---|
| 753 | } |
---|
| 754 | else if (id==3) { |
---|
| 755 | tc = (1.0f/((resx-1))*(ix+1)); |
---|
| 756 | } |
---|
| 757 | return tc; |
---|
| 758 | } |
---|
| 759 | // |
---|
| 760 | btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, |
---|
| 761 | const btVector3& radius, |
---|
| 762 | int res) |
---|
| 763 | { |
---|
| 764 | struct Hammersley |
---|
| 765 | { |
---|
| 766 | static void Generate(btVector3* x,int n) |
---|
| 767 | { |
---|
| 768 | for(int i=0;i<n;i++) |
---|
| 769 | { |
---|
| 770 | btScalar p=0.5,t=0; |
---|
| 771 | for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p; |
---|
| 772 | btScalar w=2*t-1; |
---|
| 773 | btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; |
---|
| 774 | btScalar s=btSqrt(1-w*w); |
---|
| 775 | *x++=btVector3(s*btCos(a),s*btSin(a),w); |
---|
| 776 | } |
---|
| 777 | } |
---|
| 778 | }; |
---|
| 779 | btAlignedObjectArray<btVector3> vtx; |
---|
| 780 | vtx.resize(3+res); |
---|
| 781 | Hammersley::Generate(&vtx[0],vtx.size()); |
---|
| 782 | for(int i=0;i<vtx.size();++i) |
---|
| 783 | { |
---|
| 784 | vtx[i]=vtx[i]*radius+center; |
---|
| 785 | } |
---|
| 786 | return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size())); |
---|
| 787 | } |
---|
| 788 | |
---|
| 789 | |
---|
| 790 | |
---|
| 791 | // |
---|
| 792 | btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, |
---|
| 793 | const int* triangles, |
---|
| 794 | int ntriangles) |
---|
| 795 | { |
---|
| 796 | int maxidx=0; |
---|
| 797 | int i,j,ni; |
---|
| 798 | |
---|
| 799 | for(i=0,ni=ntriangles*3;i<ni;++i) |
---|
| 800 | { |
---|
| 801 | maxidx=btMax(triangles[i],maxidx); |
---|
| 802 | } |
---|
| 803 | ++maxidx; |
---|
| 804 | btAlignedObjectArray<bool> chks; |
---|
| 805 | btAlignedObjectArray<btVector3> vtx; |
---|
| 806 | chks.resize(maxidx*maxidx,false); |
---|
| 807 | vtx.resize(maxidx); |
---|
| 808 | for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3) |
---|
| 809 | { |
---|
| 810 | vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]); |
---|
| 811 | } |
---|
| 812 | btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0); |
---|
| 813 | for( i=0,ni=ntriangles*3;i<ni;i+=3) |
---|
| 814 | { |
---|
| 815 | const int idx[]={triangles[i],triangles[i+1],triangles[i+2]}; |
---|
| 816 | #define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) |
---|
| 817 | for(int j=2,k=0;k<3;j=k++) |
---|
| 818 | { |
---|
| 819 | if(!chks[IDX(idx[j],idx[k])]) |
---|
| 820 | { |
---|
| 821 | chks[IDX(idx[j],idx[k])]=true; |
---|
| 822 | chks[IDX(idx[k],idx[k])]=true; |
---|
| 823 | psb->appendLink(idx[j],idx[k]); |
---|
| 824 | } |
---|
| 825 | } |
---|
| 826 | #undef IDX |
---|
| 827 | psb->appendFace(idx[0],idx[1],idx[2]); |
---|
| 828 | } |
---|
| 829 | psb->randomizeConstraints(); |
---|
| 830 | return(psb); |
---|
| 831 | } |
---|
| 832 | |
---|
| 833 | // |
---|
| 834 | btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, |
---|
| 835 | int nvertices) |
---|
| 836 | { |
---|
| 837 | HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); |
---|
| 838 | HullResult hres; |
---|
| 839 | HullLibrary hlib;/*??*/ |
---|
| 840 | hdsc.mMaxVertices=nvertices; |
---|
| 841 | hlib.CreateConvexHull(hdsc,hres); |
---|
| 842 | btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, |
---|
| 843 | &hres.m_OutputVertices[0],0); |
---|
| 844 | for(int i=0;i<(int)hres.mNumFaces;++i) |
---|
| 845 | { |
---|
| 846 | const int idx[]={ hres.m_Indices[i*3+0], |
---|
| 847 | hres.m_Indices[i*3+1], |
---|
| 848 | hres.m_Indices[i*3+2]}; |
---|
| 849 | if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]); |
---|
| 850 | if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]); |
---|
| 851 | if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]); |
---|
| 852 | psb->appendFace(idx[0],idx[1],idx[2]); |
---|
| 853 | } |
---|
| 854 | hlib.ReleaseResult(hres); |
---|
| 855 | psb->randomizeConstraints(); |
---|
| 856 | return(psb); |
---|
| 857 | } |
---|