Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added SoftBody demo, contribution by Nathanael Presson. Will integrate into Bullet broadphase.

Browse Source 
Added very basic drawTriangle for btIDebugDraw, useful for basic softbody visualization.
Added btGjkEpa2, contribution by Nathanael Presson. Improved version of EPA penetration depth computation, more suitable for multi-core/SPU (less memory usage). Note: btGjkEpa2 is not enabled by default currently.
This commit is contained in:
erwin.coumans
2008-03-30 23:08:06 +00:00
parent 8d38ef49ef
commit d4698cb3d5
16 changed files with 4737 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1189
src/BulletDynamics/SoftBody/btSoftBody.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

356
src/BulletDynamics/SoftBody/btSoftBody.h Normal file
View File

@@ -0,0 +1,356 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSoftBody implementation by Nathanael Presson
#ifndef _312AEEF3_52DA_4ff6_B804_FCF937182E46_
#define _312AEEF3_52DA_4ff6_B804_FCF937182E46_
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btPoint3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btIDebugDraw.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
//
// btSoftBody
//
struct btSoftBody
{
//
// Enumerations
//
/* eLType */
struct eLType { enum _ {
Structural, /* Master constraints */
Bending, /* Secondary constraints */
};};
/* eAeroModel */
struct eAeroModel { enum _ {
V_Point, /* Vertex normals are oriented toward velocity */
V_TwoSided, /* Vertex normals are fliped to match velocity */
V_OneSided, /* Vertex normals are taken as it is */
F_TwoSided, /* Face normals are fliped to match velocity */
F_OneSided, /* Face normals are taken as it is */
};};
//
// Interfaces
//
/* ISoftBody */
struct ISoftBody
{
struct sCti
{
btRigidBody* m_body; /* Rigid body */
btVector3 m_normal; /* Outward normal */
btScalar m_offset; /* Offset from origin */
};
struct sMedium
{
btVector3 m_velocity; /* Velocity */
btScalar m_pressure; /* Pressure */
btScalar m_density; /* Density */
};
virtual void Attach(btSoftBody*)
{}
virtual void Detach(btSoftBody*)
{ delete this; }
virtual void StartCollide( const btVector3& /*minbounds*/,
const btVector3& /*maxbounds*/)
{}
virtual bool CheckContact (const btVector3& /*position*/,
sCti& /*contact*/)
{ return(false); }
virtual void EndCollide()
{}
virtual void EvaluateMedium( const btVector3& /*position*/,
sMedium& medium)
{ medium.m_velocity=btVector3(0,0,0);
medium.m_pressure=0;
medium.m_density=0; }
};
//
// Internal types
//
typedef btAlignedObjectArray<btScalar> tScalarArray;
typedef btAlignedObjectArray<btVector3> tVector3Array;
/* Base type */
struct Element
{
void* m_tag; // User data
};
/* Node */
struct Node : Element
{
btVector3 m_x; // Position
btVector3 m_q; // Previous step position
btVector3 m_v; // Velocity
btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal
btScalar m_im; // 1/mass
btScalar m_area; // Area
int m_battach:1; // Attached
};
/* Link */
struct Link : Element
{
Node* m_n[2]; // Node pointers
btScalar m_rl; // Rest length
btScalar m_kST; // Stiffness coefficient
btScalar m_c0; // (ima+imb)*kLST
btScalar m_c1; // rl^2
eLType::_ m_type; // Link type
};
/* Face */
struct Face : Element
{
Node* m_n[3]; // Node pointers
btVector3 m_normal; // Normal
btScalar m_ra; // Rest area
};
/* Contact */
struct Contact
{
ISoftBody::sCti m_cti; // Contact infos
Node* m_node; // Owner node
btMatrix3x3 m_c0; // Impulse matrix
btVector3 m_c1; // Relative anchor
btScalar m_c2; // ima*dt
btScalar m_c3; // Friction
};
/* Anchor */
struct Anchor
{
Node* m_node; // Node pointer
btVector3 m_local; // Anchor position in body space
btRigidBody* m_body; // Body
btMatrix3x3 m_c0; // Impulse matrix
btVector3 m_c1; // Relative anchor
btScalar m_c2; // ima*dt
};
/* Pose */
struct Pose
{
bool m_bvolume; // Is valid
bool m_bframe; // Is frame
btScalar m_volume; // Rest volume
tVector3Array m_pos; // Reference positions
tScalarArray m_wgh; // Weights
btVector3 m_com; // COM
btMatrix3x3 m_trs; // Transform
};
/* Config */
struct Config
{
eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point)
btScalar kLST; // Linear stiffness coefficient [0,1]
btScalar kDP; // Damping coefficient [0,1]
btScalar kDG; // Drag coefficient [0,+inf]
btScalar kLF; // Lift coefficient [0,+inf]
btScalar kPR; // Pressure coefficient [-inf,+inf]
btScalar kVC; // Volume conversation coefficient [0,+inf]
btScalar kDF; // Dynamic friction coefficient [0,1]
btScalar kMT; // Pose matching coefficient [0,1]
btScalar kSOR; // SOR(w) [1,2] default 1, never use with solver!=Accurate
btScalar kCHR; // Contacts hardness [0,1]
btScalar kAHR; // Anchors hardness [0,1]
btScalar timescale; // Time scale
btScalar timestep; // Time step
int maxsteps; // Maximum time steps
int iterations; // Solver iterations
bool becollide; // Enable external collisions
bool bscollide; // Enable self collisions
};
//
// Typedef's
//
typedef btAlignedObjectArray<Node> tNodeArray;
typedef btAlignedObjectArray<Link> tLinkArray;
typedef btAlignedObjectArray<Face> tFaceArray;
typedef btAlignedObjectArray<Anchor> tAnchorArray;
typedef btAlignedObjectArray<Contact> tContactArray;
//
// Fields
//
Config m_cfg; // Configuration
Pose m_pose; // Pose
ISoftBody* m_isb; // ISoftBody
void* m_tag; // User data
tNodeArray m_nodes; // Nodes
tLinkArray m_links; // Links
tFaceArray m_faces; // Faces
tAnchorArray m_anchors; // Anchors
tContactArray m_contacts; // Contacts
btScalar m_timeacc; // Time accumulator
btVector3 m_bounds[2]; // Spatial bounds
bool m_bUpdateRtCst; // Update runtime constants
//
// Api
//
/* Create a soft body */
static btSoftBody* Create( ISoftBody* isoftbody,
int node_count,
const btVector3* x=0,
const btScalar* m=0);
/* Delete a body */
void Delete();
/* Check for existing link */
bool CheckLink( int node0,
int node1) const;
bool CheckLink( const btSoftBody::Node* node0,
const btSoftBody::Node* node1) const;
/* Check for existring face */
bool CheckFace( int node0,
int node1,
int node2) const;
/* Append link */
void AppendLink( int node0,
int node1,
btScalar kST,
btSoftBody::eLType::_ type,
bool bcheckexist=false);
void AppendLink( btSoftBody::Node* node0,
btSoftBody::Node* node1,
btScalar kST,
btSoftBody::eLType::_ type,
bool bcheckexist=false);
/* Append face */
void AppendFace( int node0,
int node1,
int node2);
/* Append anchor */
void AppendAnchor( int node,
btRigidBody* body);
/* Add force (or gravity) to the entire body */
void AddForce( const btVector3& force);
/* Add force (or gravity) to a node of the body */
void AddForce( const btVector3& force,
int node);
/* Add velocity to the entire body */
void AddVelocity( const btVector3& velocity);
/* Add velocity to a node of the body */
void AddVelocity( const btVector3& velocity,
int node);
/* Set mass */
void SetMass( int node,
btScalar mass);
/* Get mass */
btScalar GetMass( int node) const;
/* Get total mass */
btScalar GetTotalMass() const;
/* Set total mass (weighted by previous masses) */
void SetTotalMass( btScalar mass,
bool fromfaces=false);
/* Set total density */
void SetTotalDensity(btScalar density);
/* Transform */
void Transform( const btTransform& trs);
/* Scale */
void Scale( const btVector3& scl);
/* Set current state as pose */
void SetPose( bool bvolume,
bool bframe);
/* Return the volume */
btScalar GetVolume() const;
/* Generate bending constraints based on distance in the adjency graph */
int GenerateBendingConstraints( int distance,
btScalar stiffness);
/* Randomize constraints to reduce solver bias */
void RandomizeConstraints();
/* Ray casting */
btScalar Raycast( const btVector3& org,
const btVector3& dir) const;
/* Step */
void Step( btScalar dt);
};
//
// Helpers
//
/* fDrawFlags */
struct fDrawFlags { enum _ {
Nodes = 0x0001,
SLinks = 0x0002,
BLinks = 0x0004,
Faces = 0x0008,
Tetras = 0x0010,
Normals = 0x0020,
Contacts = 0x0040,
Anchors = 0x0080,
/* presets */
Links = SLinks+BLinks,
Std = SLinks+Faces+Anchors,
StdTetra = Std-Faces+Tetras,
};};
/* Draw body */
void Draw( btSoftBody* psb,
btIDebugDraw* idraw,
int drawflags=fDrawFlags::Std);
/* Draw body infos */
void DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw,
bool masses,
bool areas,
bool stress);
/* Draw rigid frame */
void DrawFrame( btSoftBody* psb,
btIDebugDraw* idraw);
/* Create a rope */
btSoftBody* CreateRope( btSoftBody::ISoftBody* isoftbody,
const btVector3& from,
const btVector3& to,
int res,
int fixeds);
/* Create a patch */
btSoftBody* CreatePatch( btSoftBody::ISoftBody* isoftbody,
const btVector3& corner00,
const btVector3& corner10,
const btVector3& corner01,
const btVector3& corner11,
int resx,
int resy,
int fixeds,
bool gendiags);
/* Create an ellipsoid */
btSoftBody* CreateEllipsoid(btSoftBody::ISoftBody* isoftbody,
const btVector3& center,
const btVector3& radius,
int res);
/* Create from convex-hull */
btSoftBody* CreateFromConvexHull( btSoftBody::ISoftBody* isoftbody,
const btVector3* vertices,
int nvertices);
/* Create from trimesh */
btSoftBody* CreateFromTriMesh( btSoftBody::ISoftBody* isoftbody,
const btScalar* vertices,
const int* triangles,
int ntriangles);
#endif

429
src/BulletDynamics/SoftBody/btSoftBodyHelpers.cpp Normal file
View File

@@ -0,0 +1,429 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSoftBodyHelpers.cpp by Nathanael Presson
#include "btSoftBody.h"
#include "Extras/ConvexHull/btConvexHull.h"
#include <stdio.h>
#include <string.h>
namespace btsoftbody_internals
{
//
static void drawVertex( btIDebugDraw* idraw,
const btVector3& x,btScalar s,const btVector3& c)
{
idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
}
//
static btVector3 stresscolor(btScalar stress)
{
static const btVector3 spectrum[]= {
btVector3(1,0,1),
btVector3(0,0,1),
btVector3(0,1,1),
btVector3(0,1,0),
btVector3(1,1,0),
btVector3(1,0,0),
btVector3(1,0,0),
};
static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
static const btScalar one=1;
stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
const int sel=(int)stress;
const btScalar frc=stress-sel;
return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
}
}
using namespace btsoftbody_internals;
//
void Draw( btSoftBody* psb,
btIDebugDraw* idraw,
int drawflags)
{
const btScalar scl=(btScalar)0.1;
const btScalar nscl=scl*5;
const btScalar alpha=(btScalar)0.5;
const btVector3 scolor=btVector3(0,0,0);
const btVector3 bcolor=btVector3(1,1,0);
const btVector3 ncolor=btVector3(1,1,1);
const btVector3 ccolor=btVector3(1,0,0);
/* Nodes */
if(0!=(drawflags&fDrawFlags::Nodes))
{
for(int i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
}
}
/* Links */
if(0!=(drawflags&fDrawFlags::Links))
{
for(int i=0;i<psb->m_links.size();++i)
{
const btSoftBody::Link& l=psb->m_links[i];
switch(l.m_type)
{
case btSoftBody::eLType::Structural:
if(0!=(drawflags&fDrawFlags::SLinks)) idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,scolor);break;
case btSoftBody::eLType::Bending:
if(0!=(drawflags&fDrawFlags::BLinks)) idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,bcolor);break;
}
}
}
/* Normals */
if(0!=(drawflags&fDrawFlags::Normals))
{
for(int i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
const btVector3 d=n.m_n*nscl;
idraw->drawLine(n.m_x,n.m_x+d,ncolor);
idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
}
}
/* Contacts */
if(0!=(drawflags&fDrawFlags::Contacts))
{
static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0),
btVector3(0,0,1)};
for(int i=0;i<psb->m_contacts.size();++i)
{
const btSoftBody::Contact& c=psb->m_contacts[i];
const btVector3 o= c.m_node->m_x-c.m_cti.m_normal*
(dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
const btVector3 x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
const btVector3 y=cross(x,c.m_cti.m_normal).normalized();
idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
}
}
/* Anchors */
if(0!=(drawflags&fDrawFlags::Anchors))
{
for(int i=0;i<psb->m_anchors.size();++i)
{
const btSoftBody::Anchor& a=psb->m_anchors[i];
const btVector3 q=a.m_body->getWorldTransform()*a.m_local;
drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
drawVertex(idraw,q,0.25,btVector3(0,1,0));
idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
}
for(int i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(n.m_im<=0)
{
drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
}
}
}
/* Faces */
if(0!=(drawflags&fDrawFlags::Faces))
{
const btScalar scl=(btScalar)0.7;
const btScalar alp=(btScalar)1;
const btVector3 col(0,(btScalar)0.7,0);
for(int i=0;i<psb->m_faces.size();++i)
{
const btSoftBody::Face& f=psb->m_faces[i];
const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
const btVector3 c=(x[0]+x[1]+x[2])/3;
idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c,
(x[2]-c)*scl+c,
f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n,
col,alp);
}
}
}
//
void DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw,
bool masses,
bool areas,
bool stress)
{
for(int i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
char text[2048]={0};
char buff[1024];
if(masses)
{
sprintf(buff," M(%.2f)",1/n.m_im);
strcat(text,buff);
}
if(areas)
{
sprintf(buff," A(%.2f)",n.m_area);
strcat(text,buff);
}
if(text[0]) idraw->draw3dText(n.m_x,text);
}
}
//
void DrawFrame( btSoftBody* psb,
btIDebugDraw* idraw)
{
if(psb->m_pose.m_bframe)
{
static const btScalar ascl=10;
static const btScalar nscl=(btScalar)0.1;
const btVector3 com=psb->m_pose.m_com;
const btMatrix3x3& trs=psb->m_pose.m_trs;
const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized();
const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized();
const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized();
idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
for(int i=0;i<psb->m_pose.m_pos.size();++i)
{
const btVector3 x=com+trs*psb->m_pose.m_pos[i];
idraw->drawLine(x-btVector3(1,0,0)*nscl,x+btVector3(1,0,0)*nscl,btVector3(1,0,1));
idraw->drawLine(x-btVector3(0,1,0)*nscl,x+btVector3(0,1,0)*nscl,btVector3(1,0,1));
idraw->drawLine(x-btVector3(0,0,1)*nscl,x+btVector3(0,0,1)*nscl,btVector3(1,0,1));
}
}
}
//
btSoftBody* CreateRope( btSoftBody::ISoftBody* isoftbody,
const btVector3& from,
const btVector3& to,
int res,
int fixeds)
{
/* Create nodes */
const int r=res+2;
btVector3* x=new btVector3[r];
btScalar* m=new btScalar[r];
for(int i=0;i<r;++i)
{
const btScalar t=i/(btScalar)(r-1);
x[i]=lerp(from,to,t);
m[i]=1;
}
btSoftBody* psb=btSoftBody::Create(isoftbody,r,x,m);
if(fixeds&1) psb->SetMass(0,0);
if(fixeds&2) psb->SetMass(r-1,0);
delete[] x;
delete[] m;
/* Create links */
for(int i=1;i<r;++i)
{
psb->AppendLink(i-1,i,1,btSoftBody::eLType::Structural);
}
/* Finished */
return(psb);
}
//
btSoftBody* CreatePatch( btSoftBody::ISoftBody* isoftbody,
const btVector3& corner00,
const btVector3& corner10,
const btVector3& corner01,
const btVector3& corner11,
int resx,
int resy,
int fixeds,
bool gendiags)
{
#define IDX(_x_,_y_) ((_y_)*rx+(_x_))
/* Create nodes */
if((resx<2)||(resy<2)) return(0);
const int rx=resx;
const int ry=resy;
const int tot=rx*ry;
btVector3* x=new btVector3[tot];
btScalar* m=new btScalar[tot];
for(int iy=0;iy<ry;++iy)
{
const btScalar ty=iy/(btScalar)(ry-1);
const btVector3 py0=lerp(corner00,corner01,ty);
const btVector3 py1=lerp(corner10,corner11,ty);
for(int ix=0;ix<rx;++ix)
{
const btScalar tx=ix/(btScalar)(rx-1);
x[IDX(ix,iy)]=lerp(py0,py1,tx);
m[IDX(ix,iy)]=1;
}
}
btSoftBody* psb=btSoftBody::Create(isoftbody,tot,x,m);
if(fixeds&1) psb->SetMass(IDX(0,0),0);
if(fixeds&2) psb->SetMass(IDX(rx-1,0),0);
if(fixeds&4) psb->SetMass(IDX(0,ry-1),0);
if(fixeds&8) psb->SetMass(IDX(rx-1,ry-1),0);
delete[] x;
delete[] m;
/* Create links and faces */
for(int iy=0;iy<ry;++iy)
{
for(int ix=0;ix<rx;++ix)
{
const int idx=IDX(ix,iy);
const bool mdx=(ix+1)<rx;
const bool mdy=(iy+1)<ry;
if(mdx) psb->AppendLink(idx,IDX(ix+1,iy),
1,btSoftBody::eLType::Structural);
if(mdy) psb->AppendLink(idx,IDX(ix,iy+1),
1,btSoftBody::eLType::Structural);
if(mdx&&mdy)
{
if((ix+iy)&1)
{
psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
psb->AppendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
if(gendiags)
{
psb->AppendLink(IDX(ix,iy),IDX(ix+1,iy+1),
1,btSoftBody::eLType::Structural);
}
}
else
{
psb->AppendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
psb->AppendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
if(gendiags)
{
psb->AppendLink(IDX(ix+1,iy),IDX(ix,iy+1),
1,btSoftBody::eLType::Structural);
}
}
}
}
}
/* Finished */
#undef IDX
return(psb);
}
//
btSoftBody* CreateEllipsoid(btSoftBody::ISoftBody* isoftbody,
const btVector3& center,
const btVector3& radius,
int res)
{
struct Hammersley
{
static void Generate(btVector3* x,int n)
{
for(int i=0;i<n;i++)
{
btScalar p=0.5,t=0;
for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
btScalar w=2*t-1;
btScalar a=(SIMD_PI+2*i*SIMD_PI)/n;
btScalar s=btSqrt(1-w*w);
*x++=btVector3(s*btCos(a),s*btSin(a),w);
}
}
};
btAlignedObjectArray<btVector3> vtx;
vtx.resize(3+res);
Hammersley::Generate(&vtx[0],vtx.size());
for(int i=0;i<vtx.size();++i)
{
vtx[i]=vtx[i]*radius+center;
}
return(CreateFromConvexHull(isoftbody,&vtx[0],vtx.size()));
}
//
btSoftBody* CreateFromConvexHull( btSoftBody::ISoftBody* isoftbody,
const btVector3* vertices,
int nvertices)
{
HullDesc hdsc(QF_TRIANGLES,nvertices,vertices);
HullResult hres;
HullLibrary hlib;/*??*/
hdsc.mMaxVertices=nvertices;
hlib.CreateConvexHull(hdsc,hres);
btSoftBody* psb=btSoftBody::Create( isoftbody,
(int)hres.mNumOutputVertices,
hres.mOutputVertices,0);
for(int i=0;i<(int)hres.mNumFaces;++i)
{
const int idx[]={ hres.mIndices[i*3+0],
hres.mIndices[i*3+1],
hres.mIndices[i*3+2]};
if(idx[0]<idx[1]) psb->AppendLink( idx[0],idx[1],
1,btSoftBody::eLType::Structural);
if(idx[1]<idx[2]) psb->AppendLink( idx[1],idx[2],
1,btSoftBody::eLType::Structural);
if(idx[2]<idx[0]) psb->AppendLink( idx[2],idx[0],
1,btSoftBody::eLType::Structural);
psb->AppendFace(idx[0],idx[1],idx[2]);
}
hlib.ReleaseResult(hres);
psb->RandomizeConstraints();
return(psb);
}
//
btSoftBody* CreateFromTriMesh( btSoftBody::ISoftBody* isoftbody,
const btScalar* vertices,
const int* triangles,
int ntriangles)
{
int maxidx=0;
for(int i=0,ni=ntriangles*3;i<ni;++i)
{
maxidx=btMax(triangles[i],maxidx);
}
++maxidx;
btAlignedObjectArray<bool> chks;
btAlignedObjectArray<btVector3> vtx;
chks.resize(maxidx*maxidx,false);
vtx.resize(maxidx);
for(int i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
{
vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
}
btSoftBody* psb=btSoftBody::Create(isoftbody,vtx.size(),&vtx[0],0);
for(int i=0,ni=ntriangles*3;i<ni;i+=3)
{
const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
for(int j=2,k=0;k<3;j=k++)
{
if(!chks[IDX(idx[j],idx[k])])
{
chks[IDX(idx[j],idx[k])]=true;
chks[IDX(idx[k],idx[k])]=true;
psb->AppendLink(idx[j],idx[k],1,btSoftBody::eLType::Structural);
}
}
#undef IDX
psb->AppendFace(idx[0],idx[1],idx[2]);
}
psb->RandomizeConstraints();
return(psb);
}

313
src/BulletDynamics/SoftBody/btSparseSDF.h Normal file
View File

@@ -0,0 +1,313 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSparseSdf implementation by Nathanael Presson
#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
template <const int CELLSIZE>
struct btSparseSdf
{
//
// Inner types
//
struct IntFrac
{
int b;
int i;
btScalar f;
};
struct Client
{
btCollisionShape* shape;
btVector3 center;
btVector3 extent;
btScalar vsize;
int id;
};
struct Cell
{
btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
int c[3];
int puid;
unsigned hash;
const Client* pclient;
Cell* next;
};
//
// Fields
//
btAlignedObjectArray<Cell*> cells;
btAlignedObjectArray<Client*> clients;
int puid;
int ncells;
int nprobes;
int nqueries;
//
// Methods
//
//
void Initialize(int hashsize=2383)
{
cells.resize(hashsize,0);
Reset();
}
//
void Reset()
{
for(int i=0,ni=cells.size();i<ni;++i)
{
Cell* pc=cells[i];
cells[i]=0;
while(pc)
{
Cell* pn=pc->next;
delete pc;
pc=pn;
}
}
puid =0;
ncells =0;
nprobes =1;
nqueries =1;
}
//
void GarbageCollect(int lifetime=64)
{
const int life=puid-lifetime;
for(int i=0;i<cells.size();++i)
{
Cell*& root=cells[i];
Cell* pp=0;
Cell* pc=root;
while(pc)
{
Cell* pn=pc->next;
if(pc->puid<life)
{
if(pp) pp->next=pn; else root=pn;
delete pc;pc=pp;--ncells;
}
pp=pc;pc=pn;
}
}
//printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
nqueries=1;
nprobes=1;
++puid; /* TODO: Reset puid's when int range limit is reached */
/* else setup a priority list... */
}
//
Client* GetClient( btCollisionShape* shape)
{
Client* pc=(Client*)shape->getUserPointer();
if(!pc)
{
pc=new Client();
clients.push_back(pc);
shape->setUserPointer(pc);
pc->shape = shape;
pc->id = clients.size();
pc->vsize = 0.25;
SetShapeBounds(*pc);
}
return(pc);
}
//
btScalar Evaluate( const btVector3& x,
btCollisionShape* shape,
btVector3& normal)
{
const Client* pclient=GetClient(shape);
/* Bounds check */
const btVector3 offset=x-pclient->center;
const btVector3 sqoffset=offset*offset;
if( (sqoffset.x()>pclient->extent.x()) ||
(sqoffset.y()>pclient->extent.y()) ||
(sqoffset.z()>pclient->extent.z())) return(SIMD_INFINITY);
/* Lookup cell */
const btVector3 scx=x/pclient->vsize;
const IntFrac ix=Decompose(scx.x());
const IntFrac iy=Decompose(scx.y());
const IntFrac iz=Decompose(scx.z());
const unsigned h=Hash(ix.b,iy.b,iz.b,pclient->id);
Cell*& root=cells[h%cells.size()];
Cell* c=root;
++nqueries;
while(c)
{
++nprobes;
if( (c->hash==h) &&
(c->c[0]==ix.b) &&
(c->c[1]==iy.b) &&
(c->c[2]==iz.b) &&
(c->pclient==pclient))
{ break; }
else
{ c=c->next; }
}
if(!c)
{
++nprobes;
++ncells;
c=new Cell();
c->next=root;root=c;
c->pclient=pclient;c->hash=h;
c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
BuildCell(*c);
}
c->puid=puid;
/* Extract infos */
const int o[]={ ix.i,iy.i,iz.i};
const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0],
c->d[o[0]+1][o[1]+0][o[2]+0],
c->d[o[0]+1][o[1]+1][o[2]+0],
c->d[o[0]+0][o[1]+1][o[2]+0],
c->d[o[0]+0][o[1]+0][o[2]+1],
c->d[o[0]+1][o[1]+0][o[2]+1],
c->d[o[0]+1][o[1]+1][o[2]+1],
c->d[o[0]+0][o[1]+1][o[2]+1]};
/* Normal */
#if 1
const btScalar gx[]={ d[1]-d[0],d[2]-d[3],
d[5]-d[4],d[6]-d[7]};
const btScalar gy[]={ d[3]-d[0],d[2]-d[1],
d[7]-d[4],d[6]-d[5]};
const btScalar gz[]={ d[4]-d[0],d[5]-d[1],
d[7]-d[3],d[6]-d[2]};
normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f),
Lerp(gx[2],gx[3],iy.f),iz.f));
normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f),
Lerp(gy[2],gy[3],ix.f),iz.f));
normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f),
Lerp(gz[2],gz[3],ix.f),iy.f));
normal = normal.normalized();
#else
normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
#endif
/* Distance */
const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f),
Lerp(d[3],d[2],ix.f),iy.f);
const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f),
Lerp(d[7],d[6],ix.f),iy.f);
return(Lerp(d0,d1,iz.f));
}
//
void BuildCell(Cell& c)
{
const Client* client=c.pclient;
const btVector3 org=btVector3(c.c[0],c.c[1],c.c[2])*CELLSIZE*client->vsize;
for(int k=0;k<=CELLSIZE;++k)
{
const btScalar z=client->vsize*k+org.z();
for(int j=0;j<=CELLSIZE;++j)
{
const btScalar y=client->vsize*j+org.y();
for(int i=0;i<=CELLSIZE;++i)
{
const btScalar x=client->vsize*i+org.x();
c.d[i][j][k]=DistanceToShape( btVector3(x,y,z),
client->shape,
client->vsize);
}
}
}
}
//
static inline btScalar DistanceToShape(const btVector3& x,
btCollisionShape* shape,
btScalar margin)
{
btTransform unit;
unit.setIdentity();
if(shape->isConvex())
{
btGjkEpaSolver2::sResults res;
btConvexShape* csh=static_cast<btConvexShape*>(shape);
return(btGjkEpaSolver2::SignedDistance(x,margin,csh,unit,res));
}
return(0);
}
//
static inline void SetShapeBounds(Client& c)
{
if(c.shape->isConvex())
{
btConvexShape* csh=static_cast<btConvexShape*>(c.shape);
const btVector3 x[]={ csh->localGetSupportingVertex(btVector3(+1,0,0)),
csh->localGetSupportingVertex(btVector3(-1,0,0))};
const btVector3 y[]={ csh->localGetSupportingVertex(btVector3(0,+1,0)),
csh->localGetSupportingVertex(btVector3(0,-1,0))};
const btVector3 z[]={ csh->localGetSupportingVertex(btVector3(0,0,+1)),
csh->localGetSupportingVertex(btVector3(0,0,-1))};
c.center = btVector3( x[0].x()+x[1].x(),
y[0].y()+y[1].y(),
z[0].z()+z[1].z())*0.5;
c.extent = btVector3( x[0].x()-x[1].x(),
y[0].y()-y[1].y(),
z[0].z()-z[1].z())*0.5;
c.extent += btVector3(c.vsize,c.vsize,c.vsize);
c.extent *= c.extent;
}
}
//
static inline IntFrac Decompose(btScalar x)
{
/* That one need a lot of improvements... */
/* Remove test, faster floor... */
IntFrac r;
x/=CELLSIZE;
const int o=x<0?(int)(-x+1):0;
x+=o;r.b=(int)x;
const btScalar k=(x-r.b)*CELLSIZE;
r.i=(int)k;r.f=k-r.i;r.b-=o;
return(r);
}
//
static inline btScalar Lerp(btScalar a,btScalar b,btScalar t)
{
return(a+(b-a)*t);
}
//
static inline unsigned Hash(int x,int y,int z,int i)
{
const int data[]={x,y,z,i};
return(HsiehHash<sizeof(data)/4>(data));
}
// Modified Paul Hsieh hash
template <const int DWORDLEN>
static inline unsigned HsiehHash(const void* pdata)
{
const unsigned short* data=(const unsigned short*)pdata;
unsigned hash=DWORDLEN<<2,tmp;
for(int i=0;i<DWORDLEN;++i)
{
hash += data[0];
tmp = (data[1]<<11)^hash;
hash = (hash<<16)^tmp;
data += 2;
hash += hash>>11;
}
hash^=hash<<3;hash+=hash>>5;
hash^=hash<<4;hash+=hash>>17;
hash^=hash<<25;hash+=hash>>6;
return(hash);
}
};
#endif