static SIMD_FORCE_INLINE void segmentsClosestPoints(
btVector3& ptsVector,
btVector3& offsetA,
btVector3& offsetB,
btScalar& tA, btScalar& tB,
const btVector3& translation,
const btVector3& dirA, btScalar hlenA,
const btVector3& dirB, btScalar hlenB )
{
// compute the parameters of the closest points on each line segment
btScalar dirA_dot_dirB = btDot(dirA,dirB);
btScalar dirA_dot_trans = btDot(dirA,translation);
btScalar dirB_dot_trans = btDot(dirB,translation);
btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
if ( denom == 0.0f ) {
tA = 0.0f;
} else {
tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
}
tB = tA * dirA_dot_dirB - dirB_dot_trans;
if ( tB < -hlenB ) {
tB = -hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
} else if ( tB > hlenB ) {
tB = hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
}
// compute the closest points relative to segment centers.
offsetA = dirA * tA;
offsetB = dirB * tB;
ptsVector = translation - offsetA + offsetB;
}
int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
{
btAssert(count);
int m=-1;
for(int i=0;i<count;i++)
if(allow[i])
{
if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
m=i;
}
btAssert(m!=-1);
return m;
}
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);
}
int EXPORT_API ProcessSpheresCollisionsVsStaticMesh(IBroadphase* tree, btVector3* positions, Triangle* triangles, btVector3* sphereCentres, float radius, int pointsCount, bool flipNormals, btVector3* colResp, int* colissionCount, bool twoSided)
{
int totalCollisions = 0;
CollisionResult collisionResults[16];
for (int k = 0; k < pointsCount; k++)
{
int colCount = IntersectSphere(tree, positions, triangles, &sphereCentres[k], radius, flipNormals, collisionResults, 16);
colissionCount[k] += colCount;
if (colCount > 0)
{
btVector3 responseVec(0, 0, 0);
for (int i = 0; i < colCount; i++)
{
CollisionResult cr = collisionResults[i];
//two sided collisions
float s = btDot(cr.contactPoint - sphereCentres[k], cr.normal);
if (twoSided && s > 0)
{
cr.normal = -cr.normal;
}
responseVec += cr.normal * (radius - cr.distance + 0.00001f);
totalCollisions++;
}
colResp[k] += responseVec / (btScalar)colCount;
}
}
return totalCollisions;
}
void EXPORT_API ComputeSphereCollisionsDEM(btVector3* positions, btVector3* velocities, btVector3* forces, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float radius, float spring, float damp, float shear, float attraction)
{
float radiusSum = radius + radius;
float radiusSumSq = radiusSum * radiusSum;
#pragma omp parallel
{
#pragma omp for
for (int idA = 0; idA < pointsCount; idA++)
{
btVector3 force(0, 0, 0);
for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
{
int idB = neighbours[idA * maxNeighboursPerPoint + nId];
// calculate relative position
btVector3 relPos = positions[idB] - positions[idA];
float distanceSq = relPos.length2();
if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
continue;
float dist = btSqrt(distanceSq);
//btScalar w1 = posLocks[idA] ? 0.0f : massInv;
//btScalar w2 = posLocks[idB] ? 0.0f : massInv;
float collideDist = radiusSum;
if (dist < collideDist)
{
btVector3 norm = relPos / dist;
// relative velocity
btVector3 relVel = velocities[idB] - velocities[idA];
// relative tangential velocity
btVector3 tanVel = relVel - (btDot(relVel, norm) * norm);
force += -spring*(collideDist - dist) * norm;
force += damp*relVel;
force += shear*tanVel;
force += attraction*relPos;
}
}
forces[idA] += force;
}
}
}
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)));
}
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;
}
static void split( const tNodeArray& leaves,
tNodeArray& left,
tNodeArray& right,
const btVector3& org,
const btVector3& axis)
{
left.resize(0);
right.resize(0);
for(int i=0,ni=leaves.size();i<ni;++i)
{
if(btDot(axis,leaves[i]->volume.Center()-org)<0)
left.push_back(leaves[i]);
else
right.push_back(leaves[i]);
}
}
void EXPORT_API ComputeViscosity(btVector3* predictedPositions, btVector3* velocities, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float KPOLY, float H, float C, 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 visc = 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 dir = predPosA - predPosB;
btVector3 velB = velocities[idB];
btVector3 velocityDiff = velB - velA;
//velocityDiff *= WPoly6(predPosA, predPosB, KPOLY, H);
velocityDiff *= WPoly6(btDot(dir, dir), KPOLY, H);
visc += velocityDiff;
}
temp[idA] = visc * C;
}
#pragma omp for
for (int i = 0; i < pointsCount; i++)
{
velocities[i] += temp[i] * dt;
}
}
}
int EXPORT_API ProjectSoftbodyCollisionConstraints(IBroadphase* tree, btVector3* predPosA, float radius, int pointsCount, float KsA, btVector3* predPosB, Triangle* triangles, float KsB, bool twoSidedCollisions)
{
int collisionTriIds[MAX_COLCOUNT];
int narrowPhaseColCount = 0;
btScalar radiusSq = radius * radius;
btScalar invMass = 1.0f;
//#pragma omp parallel for
for (int k = 0; k < pointsCount; k++)
{
btVector3 totalResponse(0, 0, 0);
int broadphaseColCount = tree->IntersectSphere(predPosA[k], radius, collisionTriIds);
for (int i = 0; i < broadphaseColCount; i++)
{
Triangle tri = triangles[collisionTriIds[i]];
const btVector3 pA = predPosB[tri.pointAid];
const btVector3 pB = predPosB[tri.pointBid];
const btVector3 pC = predPosB[tri.pointCid];
btVector3 bar;
Barycentric2(pA, pB, pC, predPosA[k], bar);
btVector3 contactPoint = pA * bar.x() + pB * bar.y() + pC * bar.z();
btVector3 triNormal;
calculateNormal(pA, pB, pC, triNormal);
btVector3 normal = predPosA[k] - contactPoint;
float distSq = normal.length2();
if (distSq > radiusSq)
continue;
float dist = btSqrt(distSq);
if (dist < 0.001f)
continue;
normal /= dist;
if (!twoSidedCollisions && btDot(normal, triNormal) < 0)
{
normal = -normal;
continue;
}
btScalar C = radius - dist;
btScalar s = invMass + invMass * bar.x() * bar.x() + invMass * bar.y() * bar.y() + invMass * bar.z() * bar.z();
if (s == 0.0f)
return narrowPhaseColCount;
btVector3 responseVec = normal * C / s;
predPosA[k] += responseVec * KsA;
predPosB[tri.pointAid] -= responseVec * bar.x() * KsB;
predPosB[tri.pointBid] -= responseVec * bar.y() * KsB;
predPosB[tri.pointCid] -= responseVec * bar.z() * KsB;
totalResponse += responseVec;
narrowPhaseColCount++;
}
//if (narrowPhaseColCount > 0)
//{
// predPosA[k] += totalResponse / (btScalar)narrowPhaseColCount;
//}
}
return narrowPhaseColCount;
}
static btDbvtNode* topdown(btDbvt* pdbvt,
tNodeArray& leaves,
int bu_treshold)
{
static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0),
btVector3(0,0,1)};
if(leaves.size()>1)
{
if(leaves.size()>bu_treshold)
{
const btDbvtVolume vol=bounds(leaves);
const btVector3 org=vol.Center();
tNodeArray sets[2];
int bestaxis=-1;
int bestmidp=leaves.size();
int splitcount[3][2]={{0,0},{0,0},{0,0}};
int i;
for( i=0;i<leaves.size();++i)
{
const btVector3 x=leaves[i]->volume.Center()-org;
for(int j=0;j<3;++j)
{
++splitcount[j][btDot(x,axis[j])>0?1:0];
}
}
for( i=0;i<3;++i)
{
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
{
const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
if(midp<bestmidp)
{
bestaxis=i;
bestmidp=midp;
}
}
}
if(bestaxis>=0)
{
sets[0].reserve(splitcount[bestaxis][0]);
sets[1].reserve(splitcount[bestaxis][1]);
split(leaves,sets[0],sets[1],org,axis[bestaxis]);
}
else
{
sets[0].reserve(leaves.size()/2+1);
sets[1].reserve(leaves.size()/2);
for(int i=0,ni=leaves.size();i<ni;++i)
{
sets[i&1].push_back(leaves[i]);
}
}
btDbvtNode* node=createnode(pdbvt,0,vol,0);
node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
node->childs[0]->parent=node;
node->childs[1]->parent=node;
return(node);
}
else
{
bottomup(pdbvt,leaves);
return(leaves[0]);
}
}
return(leaves[0]);
}
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:
{
}
}
}
}
}
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 ProjectInternalCollisionConstraintsWithFrictionMT(btVector3* positions, btVector3* predictedPositions, bool* posLocks, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float Ks_prime, float radius, float mass, float staticFric, float kineticFric)
{
btScalar massInv = 1.0f / mass;
// float Ks_prime = 1.0f - Mathf.Pow((1.0f - Ks), 1.0f / solverIterations);
float radiusSum = radius + radius;
//float radiusSum = radius * 2.0001 ;
float radiusSumSq = radiusSum * radiusSum + 0.0001f;
#pragma omp parallel
{
#pragma omp for
for (int idA = 0; idA < pointsCount; idA++)
{
btVector3 deltaP(0, 0, 0);
int collisionsCount = 0;
btVector3 posA = predictedPositions[idA];
for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
{
int idB = neighbours[idA * maxNeighboursPerPoint + nId];
btVector3 posB = predictedPositions[idB];
btVector3 dir = posA - posB;
float distanceSq = dir.length2();
if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
continue;
float distance = btSqrt(distanceSq);
btScalar w1 = posLocks[idA] ? 0.0f : massInv;
btScalar w2 = posLocks[idB] ? 0.0f : massInv;
float invMassSum = w1 + w2;
float scale = (distance - radiusSum) / invMassSum * Ks_prime;
btVector3 dP = scale * (dir / distance);
btVector3 dPA = -dP * w1 / (btScalar)pointNeighboursCount[idA];
// btVector3 dPB = dP * w2 / pointNeighboursCount[idA]; //???
btVector3 dPB = dP * w2 / (btScalar)pointNeighboursCount[idB]; //???
deltaP += dPA;
collisionsCount++;
// uint neighborIndex = gridParticleIndex[neighbors[index * MAX_FLUID_NEIGHBORS + i]];
// float3 prevPos2 = make_float3(prevPositions[neighborIndex]);
// float3 currPos2 = make_float3(newPos[neighbors[index * MAX_FLUID_NEIGHBORS + i]]);
// float3 nf = normalize(diff);
//float3 dpt = dpRel - dot(dpRel, nf) * nf;
//float ldpt = length(dpt);
//if (ldpt < EPS)
// continue;
//if (ldpt < (S_FRICTION)* dist)
// delta -= dpt * colW / (colW + colW2);
//else
// delta -= dpt * min((K_FRICTION)* dist / ldpt, 1.f);
// Apply friction
btVector3 nf = dir / distance;
// float3 dpRel = (pos + dp1 - prevPos) - (prevPos + dp2 - prevPos2);
// btVector3 dpf = (predictedPositions[idA] - positions[idA]) - (predictedPositions[idB] - positions[idB]);
btVector3 dpRel = (posA + dPA - positions[idA]) - (posB + dPB - positions[idB]);
btVector3 dpt = dpRel - btDot(dpRel, nf) * nf;
float d = radiusSum - distance;
//float d = distance;
float ldpt = dpt.length();
if (ldpt < FLT_EPSILON)
continue;
if (ldpt < staticFric * d)
deltaP -= dpt * w1 / invMassSum;
else
deltaP -= dpt * btMin(kineticFric * d / ldpt, 1.0f) * w1 / invMassSum;
// deltaP -= dpt * btMin(kineticFric * distance / ldpt, 1.0f);
}
temp[idA] = deltaP;
//if (collisionsCount > 0)
// temp[idA] = deltaP / (btScalar)collisionsCount;
//// temp[idA] = deltaP;
//else
// temp[idA] = btVector3(0, 0, 0);
}
#pragma omp for
for (int i = 0; i < pointsCount; i++)
{
predictedPositions[i] += temp[i];
}
}
}
int EXPORT_API ProcessSpheresCollisionsVsMovingMesh(IBroadphase* tree, btVector3* posA, btVector3* predPosA, float radius, int pointsCount, btVector3* posB, btVector3* predPosB, Triangle* triangles, bool twoSidedCollisions, float staticFric, float kineticFric)
{
int collisionTriIds[MAX_COLCOUNT];
int narrowPhaseColCount = 0;
btScalar radiusSq = radius * radius;
btScalar invMass = 1.0f;
//#pragma omp parallel for
for (int k = 0; k < pointsCount; k++)
{
btVector3 totalResponse(0, 0, 0);
int broadphaseColCount = tree->IntersectSphere(predPosA[k], radius, collisionTriIds);
for (int i = 0; i < broadphaseColCount; i++)
{
Triangle tri = triangles[collisionTriIds[i]];
const btVector3 pA = predPosB[tri.pointAid];
const btVector3 pB = predPosB[tri.pointBid];
const btVector3 pC = predPosB[tri.pointCid];
btVector3 bar;
Barycentric2(pA, pB, pC, predPosA[k], bar);
btVector3 contactPoint = pA * bar.x() + pB * bar.y() + pC * bar.z();
btVector3 triNormal;
calculateNormal(pA, pB, pC, triNormal);
btVector3 normal = predPosA[k] - contactPoint;
float distSq = normal.length2();
if (distSq > radiusSq)
continue;
float dist = btSqrt(distSq);
if (dist < 0.00001f)
continue;
normal /= dist;
if (!twoSidedCollisions && btDot(normal, triNormal) < 0)
{
normal = -normal;
continue;
}
btScalar C = radius - dist;
btVector3 responseVec = normal * C;
predPosA[k] += responseVec;
//FRICTION
btVector3 contactPointPred = predPosB[tri.pointAid] * bar.x() + predPosB[tri.pointBid] * bar.y() + predPosB[tri.pointCid] * bar.z();
btVector3 contactPointInitial = posB[tri.pointAid] * bar.x() + posB[tri.pointBid] * bar.y() + posB[tri.pointCid] * bar.z();
btVector3 dpf = (predPosA[k] - posA[k]) - (contactPointPred - contactPointInitial);
btVector3 dpt = dpf - btDot(dpf, normal) * normal;
float ldpt = dpt.length();
if (ldpt < 0.000001f)
continue;
if (ldpt < staticFric * C)
predPosA[k] -= dpt;
else
predPosA[k] -= dpt * btMin(kineticFric * C / ldpt, 1.0f);
totalResponse += responseVec;
narrowPhaseColCount++;
}
}
return narrowPhaseColCount;
}
int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon)
{
btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
}
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;
}
}
}
int PlaneTest(const btPlane &p, const btVector3 &v) {
btScalar a = btDot(v,p.normal)+p.dist;
int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
return flag;
}
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
{
return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
}
int EXPORT_API ProjectSoftbodyCollisionConstraintsWithFriction(IBroadphase* tree, btVector3* posA, btVector3* predPosA, float radius, int pointsCount, float KsA, btVector3* posB, btVector3* predPosB, Triangle* triangles, float KsB, bool twoSidedCollisions, float staticFric, float kineticFric)
{
int collisionTriIds[MAX_COLCOUNT];
int narrowPhaseColCount = 0;
btScalar radiusSq = radius * radius;
btScalar invMass = 1.0f;
//#pragma omp parallel for
for (int k = 0; k < pointsCount; k++)
{
btVector3 totalResponse(0, 0, 0);
int broadphaseColCount = tree->IntersectSphere(predPosA[k], radius, collisionTriIds);
for (int i = 0; i < broadphaseColCount; i++)
{
Triangle tri = triangles[collisionTriIds[i]];
const btVector3 pA = predPosB[tri.pointAid];
const btVector3 pB = predPosB[tri.pointBid];
const btVector3 pC = predPosB[tri.pointCid];
btVector3 bar;
Barycentric2(pA, pB, pC, predPosA[k], bar);
btVector3 contactPoint = pA * bar.x() + pB * bar.y() + pC * bar.z();
btVector3 triNormal;
calculateNormal(pA, pB, pC, triNormal);
btVector3 normal = predPosA[k] - contactPoint;
float distSq = normal.length2();
if (distSq > radiusSq)
continue;
float dist = btSqrt(distSq);
if (dist < 0.00001f)
continue;
normal /= dist;
if (!twoSidedCollisions && btDot(normal, triNormal) < 0)
{
normal = -normal;
continue;
}
btScalar C = radius - dist;
btScalar s = invMass + invMass * bar.x() * bar.x() + invMass * bar.y() * bar.y() + invMass * bar.z() * bar.z();
if (s == 0.0f)
return narrowPhaseColCount;
btVector3 responseVec = normal * C / s;
predPosA[k] += responseVec * KsA;
predPosB[tri.pointAid] -= responseVec * bar.x() * KsB;
predPosB[tri.pointBid] -= responseVec * bar.y() * KsB;
predPosB[tri.pointCid] -= responseVec * bar.z() * KsB;
// Vector4 contactPointPred = contactPoint - responseVec;
btVector3 contactPointPred = predPosB[tri.pointAid] * bar.x() + predPosB[tri.pointBid] * bar.y() + predPosB[tri.pointCid] * bar.z();
btVector3 contactPointInitial = posB[tri.pointAid] * bar.x() + posB[tri.pointBid] * bar.y() + posB[tri.pointCid] * bar.z();
btVector3 nf = normal;
btVector3 dpf = (predPosA[k] - posA[k]) - (contactPointPred - contactPointInitial);
btVector3 dpt = dpf - btDot(dpf, nf) * nf;
// float d = radiusSum - distance;
float ldpt = dpt.length();
if (ldpt < 0.00001f)
continue;
if (ldpt < staticFric * C)
{
// Vector3 delta = dpt / invMassSum;
btVector3 delta = dpt / s;
predPosA[k] -= delta * invMass * KsA;
predPosB[tri.pointAid] += delta * bar.x() * invMass * KsB;
predPosB[tri.pointBid] += delta * bar.y() * invMass * KsB;
predPosB[tri.pointCid] += delta * bar.z() * invMass * KsB;
}
else
{
// Vector3 delta = dpt * Mathf.min(kineticFric * d / ldpt, 1.0f) / invMassSum;
btVector3 delta = dpt * btMin(kineticFric * C / ldpt, 1.0f) / s;
predPosA[k] -= delta * invMass * KsA;
predPosB[tri.pointAid] += delta * bar.x() * invMass * KsB;
predPosB[tri.pointBid] += delta * bar.y() * invMass * KsB;
predPosB[tri.pointCid] += delta * bar.z() * invMass * KsB;
}
totalResponse += responseVec;
narrowPhaseColCount++;
}
}
return narrowPhaseColCount;
}
bool ProjectEdgeEdgeDistConstraint(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& p3,
float invMass0, float invMass1, float invMass2, float invMass3, float restDist,
btVector3& corr0, btVector3& corr1, btVector3& corr2, btVector3& corr3)
{
btVector3 d0 = p1 - p0;
btVector3 d1 = p3 - p2;
float a = btDot(d0, d0);
float b = btDot(-d0, d1);
float c = btDot(d0, d1);
float d = -btDot(-d1, -d1);
float e = btDot(p2 - p0, d0);
float f = btDot(p2 - p0, d1);
float det = a * d - b * c;
float s, t;
if (det != 0.0f)
{
det = 1.0f / det;
s = (e * d - b * f) * det;
t = (a * f - e * c) * det;
}
else
{ // d0 and d1 parallel
float s0 = btDot(p0, d0);
float s1 = btDot(p1, d0);
float t0 = btDot(p2, d0);
float t1 = btDot(p3, d0);
bool flip0 = false;
bool flip1 = false;
if (s0 > s1) {
float f1 = s0;
s0 = s1;
s1 = f1;
flip0 = true;
}
if (t0 > t1) {
float f2 = t0;
t0 = t1;
t1 = f2;
flip1 = true;
}
if (s0 >= t1)
{
s = !flip0 ? 0.0f : 1.0f;
t = !flip1 ? 1.0f : 0.0f;
}
else if (t0 >= s1)
{
s = !flip0 ? 1.0f : 0.0f;
t = !flip1 ? 0.0f : 1.0f;
}
else
{ // overlap
float mid = (s0 > t0) ? (s0 + t1) * 0.5f : (t0 + s1) * 0.5f;
s = (s0 == s1) ? 0.5f : (mid - s0) / (s1 - s0);
t = (t0 == t1) ? 0.5f : (mid - t0) / (t1 - t0);
}
}
if (s < 0.0f) s = 0.0f;
if (s > 1.0f) s = 1.0f;
if (t < 0.0f) t = 0.0f;
if (t > 1.0f) t = 1.0f;
float b0 = 1.0f - s;
float b1 = s;
float b2 = 1.0f - t;
float b3 = t;
btVector3 q0 = p0 * b0 + p1 * b1;
btVector3 q1 = p2 * b2 + p3 * b3;
btVector3 n = q0 - q1;
float dist = n.length();
if (dist > restDist)
return false;
n.normalize();
float C = dist - restDist;
btVector3 grad0 = n * b0;
btVector3 grad1 = n * b1;
btVector3 grad2 = -n * b2;
btVector3 grad3 = -n * b3;
s = invMass0 * b0 * b0 + invMass1 * b1 * b1 + invMass2 * b2 * b2 + invMass3 * b3 * b3;
if (s == 0.0f)
return false;
s = C / s;
if (s == 0.0f)
return false;
corr0 = -s * invMass0 * grad0;
corr1 = -s * invMass1 * grad1;
corr2 = -s * invMass2 * grad2;
corr3 = -s * invMass3 * grad3;
return true;
}
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 ProjectInternalCollisionConstraintsWithFriction(btVector3* positions, btVector3* predictedPositions, bool* posLocks, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float Ks_prime, float radius, float mass, float staticFric, float kineticFric)
{
btScalar massInv = 1.0f / mass;
// float Ks_prime = 1.0f - Mathf.Pow((1.0f - Ks), 1.0f / solverIterations);
float radiusSum = radius + radius;
float radiusSumSq = radiusSum * radiusSum;
//#pragma omp parallel for
for (int idA = 0; idA < pointsCount; idA++)
{
// int collisionsCount = 0;
for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
{
int idB = neighbours[idA * maxNeighboursPerPoint + nId];
btVector3 dir = predictedPositions[idA] - predictedPositions[idB];
float distanceSq = dir.length2();
if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
continue;
float distance = btSqrt(distanceSq);
btScalar w1 = posLocks[idA] ? 0.0f : massInv;
btScalar w2 = posLocks[idB] ? 0.0f : massInv;
float invMassSum = w1 + w2;
btVector3 dP = (1.0f / invMassSum) * (distance - radiusSum) * (dir / distance) * Ks_prime;
predictedPositions[idA] -= dP * w1;
predictedPositions[idB] += dP * w2;
// Apply friction
btVector3 nf = dir / distance;
btVector3 dpf = (predictedPositions[idA] - positions[idA]) - (predictedPositions[idB] - positions[idB]);
btVector3 dpt = dpf - btDot(dpf, nf) * nf;
float d = radiusSum - distance;
float ldpt = dpt.length();
if (ldpt < FLT_EPSILON)
continue;
if (ldpt < staticFric * d)
{
btVector3 delta = dpt / invMassSum;
predictedPositions[idA] -= delta * w1;
predictedPositions[idB] += delta * w2;
}
else
{
btVector3 delta = dpt * btMin(kineticFric * d / ldpt, 1.0f) / invMassSum;
predictedPositions[idA] -= delta * w1;
predictedPositions[idB] += delta * w2;
}
}
}
}
