int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
{
btVector3 basis[3];
basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );
int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
basis[0] = verts[p0]-verts[p1];
if(p0==p1 || basis[0]==btVector3(0,0,0))
return int4(-1,-1,-1,-1);
basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]);
if (basis[1].length() > basis[2].length())
{
basis[1].normalize();
} else {
basis[1] = basis[2];
basis[1].normalize ();
}
int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
if(p2 == p0 || p2 == p1)
{
p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
}
if(p2 == p0 || p2 == p1)
return int4(-1,-1,-1,-1);
basis[1] = verts[p2] - verts[p0];
basis[2] = btCross(basis[1],basis[0]).normalized();
int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
if(p3==p0||p3==p1||p3==p2)
return int4(-1,-1,-1,-1);
btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
return int4(p0,p1,p2,p3);
}
void EXPORT_API ComputeVolumeConstraintsST(btVector3* predPositions, float* invMasses, bool* posLocks, Tetrahedron* tetras, btVector3* temp, int pointsCount, int tetrasCount, float Ks_prime)
{
// btScalar massInv = 1.0f / mass;
// very fast but causing visible instabilities
// #pragma omp parallel for
for (int i = 0; i < tetrasCount; i++)
{
Tetrahedron tetra = tetras[i];
if (tetra.restVolume == 0.0f)
continue;
btVector3 X0 = predPositions[tetra.idA];
btVector3 X1 = predPositions[tetra.idB];
btVector3 X2 = predPositions[tetra.idC];
btVector3 X3 = predPositions[tetra.idD];
btVector3 p1 = X1 - X0;
btVector3 p2 = X2 - X0;
btVector3 p3 = X3 - X0;
btVector3 q1 = btCross(p2, p3);
btVector3 q2 = btCross(p3, p1);
btVector3 q3 = btCross(p1, p2);
btVector3 q0 = -q1 - q2 - q3;
float volume = btDot(p1, btCross(p2, p3));
float lambda = invMasses[tetra.idA] * btDot(q0, q0)
+ invMasses[tetra.idB] * btDot(q1, q1)
+ invMasses[tetra.idC] * btDot(q2, q2)
+ invMasses[tetra.idD] * btDot(q3, q3);
if (lambda < 0.000001f)
continue;
lambda = -(volume - tetra.restVolume) / lambda * Ks_prime;
btVector3 dP1 = q0 * lambda;
btVector3 dP2 = q1 * lambda;
btVector3 dP3 = q2 * lambda;
btVector3 dP4 = q3 * lambda;
float w1 = posLocks[tetra.idA] ? 0.0f : invMasses[tetra.idA];
float w2 = posLocks[tetra.idB] ? 0.0f : invMasses[tetra.idB];
float w3 = posLocks[tetra.idC] ? 0.0f : invMasses[tetra.idC];
float w4 = posLocks[tetra.idD] ? 0.0f : invMasses[tetra.idD];
predPositions[tetra.idA] += dP1 * w1;
predPositions[tetra.idB] += dP2 * w2;
predPositions[tetra.idC] += dP3 * w3;
predPositions[tetra.idD] += dP4 * w4;
}
}
btVector3 orth(const btVector3 &v)
{
btVector3 a=btCross(v,btVector3(0,0,1));
btVector3 b=btCross(v,btVector3(0,1,0));
if (a.length() > b.length())
{
return a.normalized();
} else {
return b.normalized();
}
}
btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
{
// return the normal of the triangle
// inscribed by v0, v1, and v2
btVector3 cp=btCross(v1-v0,v2-v1);
btScalar m=cp.length();
if(m==0) return btVector3(1,0,0);
return cp*(btScalar(1.0)/m);
}
static inline btScalar tetravolume(const btVector3& x0,
const btVector3& x1,
const btVector3& x2,
const btVector3& x3)
{
const btVector3 a=x1-x0;
const btVector3 b=x2-x0;
const btVector3 c=x3-x0;
return(btDot(a,btCross(b,c)));
}
void GLDebugDrawer::drawTriangle(const btVector3 &a, const btVector3 &b,
const btVector3 &c,const btVector3 &color, btScalar alpha)
{
const btVector3 n = btCross(b - a, c - a).normalized();
Mygl gl(3, 0, true, false, true);
gl.glBegin(GL_TRIANGLES);
gl.glColor(color, alpha);
gl.glNormal(n);
gl.glVertex(a, b, c);
gl.glEnd();
}
void GLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha) {
{
const btVector3 n=btCross(b-a,c-a).normalized();
glBegin(GL_TRIANGLES);
glColor4f(color.getX(), color.getY(), color.getZ(),alpha);
glNormal3d(n.getX(),n.getY(),n.getZ());
glVertex3d(a.getX(),a.getY(),a.getZ());
glVertex3d(b.getX(),b.getY(),b.getZ());
glVertex3d(c.getX(),c.getY(),c.getZ());
glEnd();
}
}
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
{
static btVector3 cp;
cp = btCross(udir,vdir).normalized();
btScalar distu = -btDot(cp,ustart);
btScalar distv = -btDot(cp,vstart);
btScalar dist = (btScalar)fabs(distu-distv);
if(upoint)
{
btPlane plane;
plane.normal = btCross(vdir,cp).normalized();
plane.dist = -btDot(plane.normal,vstart);
*upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
}
if(vpoint)
{
btPlane plane;
plane.normal = btCross(udir,cp).normalized();
plane.dist = -btDot(plane.normal,ustart);
*vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
}
return dist;
}
int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
{
int m=-1;
while(m==-1)
{
m = maxdirfiltered(p,count,dir,allow);
if(allow[m]==3) return m;
T u = orth(dir);
T v = btCross(u,dir);
int ma=-1;
for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
{
btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
if(ma==m && mb==m)
{
allow[m]=3;
return m;
}
if(ma!=-1 && ma!=mb) // Yuck - this is really ugly
{
int mc = ma;
for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
{
btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
if(mc==m && md==m)
{
allow[m]=3;
return m;
}
mc=md;
}
}
ma=mb;
}
allow[m]=0;
m=-1;
}
btAssert(0);
return m;
}
GL_ShapeDrawer::ShapeCache* GL_ShapeDrawer::cache(btConvexShape* shape)
{
ShapeCache* sc=(ShapeCache*)shape->getUserPointer();
if(!sc)
{
sc=new(btAlignedAlloc(sizeof(ShapeCache),16)) ShapeCache(shape);
sc->m_shapehull.buildHull(shape->getMargin());
m_shapecaches.push_back(sc);
shape->setUserPointer(sc);
/* Build edges */
const int ni=sc->m_shapehull.numIndices();
const int nv=sc->m_shapehull.numVertices();
const unsigned int* pi=sc->m_shapehull.getIndexPointer();
const btVector3* pv=sc->m_shapehull.getVertexPointer();
btAlignedObjectArray<ShapeCache::Edge*> edges;
sc->m_edges.reserve(ni);
edges.resize(nv*nv,0);
for(int i=0;i<ni;i+=3)
{
const unsigned int* ti=pi+i;
const btVector3 nrm=btCross(pv[ti[1]]-pv[ti[0]],pv[ti[2]]-pv[ti[0]]).normalized();
for(int j=2,k=0;k<3;j=k++)
{
const unsigned int a=ti[j];
const unsigned int b=ti[k];
ShapeCache::Edge*& e=edges[btMin(a,b)*nv+btMax(a,b)];
if(!e)
{
sc->m_edges.push_back(ShapeCache::Edge());
e=&sc->m_edges[sc->m_edges.size()-1];
e->n[0]=nrm;e->n[1]=-nrm;
e->v[0]=a;e->v[1]=b;
}
else
{
e->n[1]=nrm;
}
}
}
}
return(sc);
}
void SingularityDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha) {
const btVector3 n = btCross(b-a,c-a).normalized();
btDebugVertex p0, p1, p2;
p0.r = color.getX();
p0.g = color.getY();
p0.b = color.getZ();
p0.x = a.getX();
p0.y = a.getY();
p0.z = a.getZ();
p0.nx = n.x();
p0.ny = n.y();
p0.nz = n.z();
p0.r = color.getX();
p0.g = color.getY();
p0.b = color.getZ();
p0.x = b.getX();
p0.y = b.getY();
p0.z = b.getZ();
p0.nx = n.x();
p0.ny = n.y();
p0.nz = n.z();
p0.r = color.getX();
p0.g = color.getY();
p0.b = color.getZ();
p0.x = c.getX();
p0.y = c.getY();
p0.z = c.getZ();
p0.nx = n.x();
p0.ny = n.y();
p0.nz = n.z();
m_trianglesVertexList.push_back(p0);
m_trianglesVertexList.push_back(p1);
m_trianglesVertexList.push_back(p2);
}
void btSoftBodyHelpers::Draw( btSoftBody* psb,
btIDebugDraw* idraw,
int drawflags)
{
const btScalar scl=(btScalar)0.1;
const btScalar nscl=scl*5;
const btVector3 lcolor=btVector3(0,0,0);
const btVector3 ncolor=btVector3(1,1,1);
const btVector3 ccolor=btVector3(1,0,0);
int i,j,nj;
/* Clusters */
if(0!=(drawflags&fDrawFlags::Clusters))
{
srand(1806);
for(i=0;i<psb->m_clusters.size();++i)
{
if(psb->m_clusters[i]->m_collide)
{
btVector3 color( rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX);
color=color.normalized()*0.75;
btAlignedObjectArray<btVector3> vertices;
vertices.resize(psb->m_clusters[i]->m_nodes.size());
for(j=0,nj=vertices.size();j<nj;++j)
{
vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
}
#define USE_NEW_CONVEX_HULL_COMPUTER
#ifdef USE_NEW_CONVEX_HULL_COMPUTER
btConvexHullComputer computer;
int stride = sizeof(btVector3);
int count = vertices.size();
btScalar shrink=0.f;
btScalar shrinkClamp=0.f;
computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp);
for (int i=0;i<computer.faces.size();i++)
{
int face = computer.faces[i];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];
const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();
int v0 = firstEdge->getSourceVertex();
int v1 = firstEdge->getTargetVertex();
while (edge!=firstEdge)
{
int v2 = edge->getTargetVertex();
idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1);
edge = edge->getNextEdgeOfFace();
v0=v1;
v1=v2;
};
}
#else
HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
HullResult hres;
HullLibrary hlib;
hdsc.mMaxVertices=vertices.size();
hlib.CreateConvexHull(hdsc,hres);
const btVector3 center=average(hres.m_OutputVertices);
add(hres.m_OutputVertices,-center);
mul(hres.m_OutputVertices,(btScalar)1);
add(hres.m_OutputVertices,center);
for(j=0;j<(int)hres.mNumFaces;++j)
{
const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
hres.m_OutputVertices[idx[1]],
hres.m_OutputVertices[idx[2]],
color,1);
}
hlib.ReleaseResult(hres);
#endif
}
/* Velocities */
#if 0
for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
{
const btSoftBody::Cluster& c=psb->m_clusters[i];
const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
const btVector3 v=c.m_lv+btCross(c.m_av,r);
idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
}
#endif
/* Frame */
// btSoftBody::Cluster& c=*psb->m_clusters[i];
// idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
// idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
// idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
}
}
else
{
/* Nodes */
if(0!=(drawflags&fDrawFlags::Nodes))
{
for(i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
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(i=0;i<psb->m_links.size();++i)
{
const btSoftBody::Link& l=psb->m_links[i];
if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
}
}
/* Normals */
if(0!=(drawflags&fDrawFlags::Normals))
{
for(i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
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(i=0;i<psb->m_rcontacts.size();++i)
{
const btSoftBody::RContact& c=psb->m_rcontacts[i];
const btVector3 o= c.m_node->m_x-c.m_cti.m_normal*
(btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
const btVector3 y=btCross(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));
}
}
/* Faces */
if(0!=(drawflags&fDrawFlags::Faces))
{
const btScalar scl=(btScalar)0.8;
const btScalar alp=(btScalar)1;
const btVector3 col(0,(btScalar)0.7,0);
for(i=0;i<psb->m_faces.size();++i)
{
const btSoftBody::Face& f=psb->m_faces[i];
if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
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,
col,alp);
}
}
/* Tetras */
if(0!=(drawflags&fDrawFlags::Tetras))
{
const btScalar scl=(btScalar)0.8;
const btScalar alp=(btScalar)1;
const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7);
for(int i=0;i<psb->m_tetras.size();++i)
{
const btSoftBody::Tetra& t=psb->m_tetras[i];
if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
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};
const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4;
idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
}
}
}
/* Anchors */
if(0!=(drawflags&fDrawFlags::Anchors))
{
for(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(i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
if(n.m_im<=0)
{
drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
}
}
}
/* Notes */
if(0!=(drawflags&fDrawFlags::Notes))
{
for(i=0;i<psb->m_notes.size();++i)
{
const btSoftBody::Note& n=psb->m_notes[i];
btVector3 p=n.m_offset;
for(int j=0;j<n.m_rank;++j)
{
p+=n.m_nodes[j]->m_x*n.m_coords[j];
}
idraw->draw3dText(p,n.m_text);
}
}
/* Node tree */
if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw);
/* Face tree */
if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw);
/* Cluster tree */
if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw);
/* Joints */
if(0!=(drawflags&fDrawFlags::Joints))
{
for(i=0;i<psb->m_joints.size();++i)
{
const btSoftBody::Joint* pj=psb->m_joints[i];
switch(pj->Type())
{
case btSoftBody::Joint::eType::Linear:
{
const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj;
const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
drawVertex(idraw,a0,0.25,btVector3(1,1,0));
drawVertex(idraw,a1,0.25,btVector3(0,1,1));
}
break;
case btSoftBody::Joint::eType::Angular:
{
//const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
break;
}
default:
{
}
}
}
}
}
float csBulletCollider::GetVolume ()
{
switch (geomType)
{
case BOX_COLLIDER_GEOMETRY:
{
csVector3 size;
GetBoxGeometry (size);
return size[0] * size[1] * size[2];
}
case SPHERE_COLLIDER_GEOMETRY:
{
csSphere sphere;
GetSphereGeometry (sphere);
return 1.333333f * PI * sphere.GetRadius () * sphere.GetRadius ()
* sphere.GetRadius ();
}
case CYLINDER_COLLIDER_GEOMETRY:
{
float length;
float radius;
GetCylinderGeometry (length, radius);
return PI * radius * radius * length;
}
case CAPSULE_COLLIDER_GEOMETRY:
{
float length;
float radius;
GetCapsuleGeometry (length, radius);
return PI * radius * radius * length
+ 1.333333f * PI * radius * radius * radius;
}
case CONVEXMESH_COLLIDER_GEOMETRY:
{
if (vertexCount == 0)
return 0.0f;
float volume = 0.0f;
int faceCount = (int)vertexCount / 3;
btVector3 origin = vertices[indices[0]];
for (int i = 1; i < faceCount; i++)
{
int index = i * 3;
volume += fabsl (btDot
(vertices[indices[index]] - origin,
btCross (vertices[indices[index + 1]] - origin,
vertices[indices[index + 2]] - origin)));
}
return volume / 6.0f;
}
case TRIMESH_COLLIDER_GEOMETRY:
{
if (vertexCount == 0)
return 0.0f;
// TODO: this is a really rough estimation
btVector3 center;
btScalar radius;
shape->getBoundingSphere (center, radius);
return 1.333333f * PI * radius * radius * radius;
}
default:
return 0.0f;
}
return 0.0f;
}
int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
{
if(verts_count <4) return 0;
if(vlimit==0) vlimit=1000000000;
int j;
btVector3 bmin(*verts),bmax(*verts);
btAlignedObjectArray<int> isextreme;
isextreme.reserve(verts_count);
btAlignedObjectArray<int> allow;
allow.reserve(verts_count);
for(j=0;j<verts_count;j++)
{
allow.push_back(1);
isextreme.push_back(0);
bmin.setMin (verts[j]);
bmax.setMax (verts[j]);
}
btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
btAssert (epsilon != 0.0);
int4 p = FindSimplex(verts,verts_count,allow);
if(p.x==-1) return 0; // simplex failed
btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point
btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
checkit(t0);checkit(t1);checkit(t2);checkit(t3);
for(j=0;j<m_tris.size();j++)
{
btHullTriangle *t=m_tris[j];
btAssert(t);
btAssert(t->vmax<0);
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
t->vmax = maxdirsterid(verts,verts_count,n,allow);
t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
}
btHullTriangle *te;
vlimit-=4;
while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
{
int3 ti=*te;
int v=te->vmax;
btAssert(v != -1);
btAssert(!isextreme[v]); // wtf we've already done this vertex
isextreme[v]=1;
//if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
j=m_tris.size();
while(j--) {
if(!m_tris[j]) continue;
int3 t=*m_tris[j];
if(above(verts,t,verts[v],btScalar(0.01)*epsilon))
{
extrude(m_tris[j],v);
}
}
// now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
j=m_tris.size();
while(j--)
{
if(!m_tris[j]) continue;
if(!hasvert(*m_tris[j],v)) break;
int3 nt=*m_tris[j];
if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
{
btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
extrude(nb,v);
j=m_tris.size();
}
}
j=m_tris.size();
while(j--)
{
btHullTriangle *t=m_tris[j];
if(!t) continue;
if(t->vmax>=0) break;
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
t->vmax = maxdirsterid(verts,verts_count,n,allow);
if(isextreme[t->vmax])
{
t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
}
else
{
t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
}
}
vlimit --;
}
return 1;
}
void EXPORT_API ComputeVolumeConstraintsMT(btVector3* predPositions, float* invMasses, bool* posLocks, Tetrahedron* tetras, btVector3* temp, int pointsCount, int tetrasCount, float Ks_prime)
{
// btScalar massInv = 1.0f / mass;
short* tempInts = new short[pointsCount];
// omp_lock_t* lock = new omp_lock_t[pointsCount];
//synchronization using locks is slower than the single threaded version
// #pragma omp parallel shared(temp, tempInts, locks)
#pragma omp parallel
{
#pragma omp for
for (int i = 0; i < pointsCount; i++)
{
temp[i] = btVector3(0,0,0);
tempInts[i] = 0;
// omp_init_lock(&(lock[i]));
}
#pragma omp for
for (int i = 0; i < tetrasCount; i++)
{
Tetrahedron tetra = tetras[i];
if (tetra.restVolume == 0.0f)
continue;
btVector3 X0 = predPositions[tetra.idA];
btVector3 X1 = predPositions[tetra.idB];
btVector3 X2 = predPositions[tetra.idC];
btVector3 X3 = predPositions[tetra.idD];
btVector3 p1 = X1 - X0;
btVector3 p2 = X2 - X0;
btVector3 p3 = X3 - X0;
btVector3 q1 = btCross(p2, p3);
btVector3 q2 = btCross(p3, p1);
btVector3 q3 = btCross(p1, p2);
btVector3 q0 = -q1 - q2 - q3;
float volume = btDot(p1, btCross(p2, p3));
float lambda = invMasses[tetra.idA] * btDot(q0, q0)
+ invMasses[tetra.idB] * btDot(q1, q1)
+ invMasses[tetra.idC] * btDot(q2, q2)
+ invMasses[tetra.idD] * btDot(q3, q3);
if (lambda < 0.000001f)
continue;
lambda = -(volume - tetra.restVolume) / lambda * Ks_prime;
btVector3 dP1 = q0 * lambda;
btVector3 dP2 = q1 * lambda;
btVector3 dP3 = q2 * lambda;
btVector3 dP4 = q3 * lambda;
float w1 = posLocks[tetra.idA] ? 0.0f : invMasses[tetra.idA];;
float w2 = posLocks[tetra.idB] ? 0.0f : invMasses[tetra.idB];;
float w3 = posLocks[tetra.idC] ? 0.0f : invMasses[tetra.idC];;
float w4 = posLocks[tetra.idD] ? 0.0f : invMasses[tetra.idD];;
//omp_set_lock(&(locks[tetra.idA]));
temp[tetra.idA] += dP1 * w1;
tempInts[tetra.idA]++;
//omp_unset_lock(&(locks[tetra.idA]));
//omp_set_lock(&(locks[tetra.idB]));
temp[tetra.idB] += dP2 * w2;
tempInts[tetra.idB]++;
//omp_unset_lock(&(locks[tetra.idB]));
//omp_set_lock(&(locks[tetra.idC]));
temp[tetra.idC] += dP3 * w3;
tempInts[tetra.idC]++;
//omp_unset_lock(&(locks[tetra.idC]));
//omp_set_lock(&(locks[tetra.idD]));
temp[tetra.idD] += dP4 * w4;
tempInts[tetra.idD]++;
//omp_unset_lock(&(locks[tetra.idD]));
// predPositions[tetra.idA] += dP1 * w1;
// predPositions[tetra.idB] += dP2 * w2;
// predPositions[tetra.idC] += dP3 * w3;
// predPositions[tetra.idD] += dP4 * w4;
}
#pragma omp for
for (int i = 0; i < pointsCount; i++)
{
if (tempInts[i] > 0)
predPositions[i] += temp[i] / (btScalar)tempInts[i];
// omp_destroy_lock(&(lock[i]));
}
}
delete tempInts;
// delete lock;
}
void EXPORT_API ComputeVorticity(btVector3* predictedPositions, btVector3* velocities, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float KPOLY, float SPIKY, float H, float EPSILON_VORTICITY, float dt)
{
#pragma omp parallel
{
#pragma omp for
for (int idA = 0; idA < pointsCount; idA++)
{
btVector3 deltaP(0, 0, 0);
btVector3 predPosA = predictedPositions[idA];
btVector3 velA = velocities[idA];
btVector3 omega = btVector3(0.0f, 0.0f, 0.0f);
btVector3 eta = btVector3(0.0f, 0.0f, 0.0f);
for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
{
int idB = neighbours[idA * maxNeighboursPerPoint + nId];
btVector3 predPosB = predictedPositions[idB];
btVector3 velB = velocities[idB];
btVector3 dir = predPosA - predPosB;
btVector3 velocityDiff = velB - velA;
btVector3 spiky = WSpiky(dir, SPIKY, H);
btVector3 gradient = spiky;
omega += btCross(velocityDiff, gradient);
eta += spiky;
}
//float omegaLength = length(omega);
float omegaLength = btDot(omega, omega);
if (omegaLength == 0.0f)
{
//No direction for eta
temp[idA] = btVector3(0.0f, 0.0f, 0.0f);
continue;
}
// float3 eta = eta(p, omegaLength);
eta *= omegaLength;
// if (eta == float3(0.0f, 0.0f, 0.0f))
if (btDot(eta, eta) == 0.0f)
{
//Particle is isolated or net force is 0
temp[idA] = btVector3(0.0f, 0.0f, 0.0f);
continue;
}
btVector3 etaNormal = eta.normalized();
if (isinf(etaNormal.x()) || isinf(etaNormal.y()) || isinf(etaNormal.z()))
{
temp[idA] = btVector3(0.0f, 0.0f, 0.0f);
//return;
continue;
}
temp[idA] = btCross(etaNormal, omega) * EPSILON_VORTICITY;
}
#pragma omp for
for (int i = 0; i < pointsCount; i++)
{
velocities[i] += temp[i] * dt;
}
}
}