Ejemplo n.º 1
0
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);
}
Ejemplo n.º 4
0
    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;
    }
Ejemplo n.º 5
0
	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;

			}
		}


	}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 8
0
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]);
	}
}
Ejemplo n.º 9
0
	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;
			}
		}
	}
Ejemplo n.º 10
0
    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;
    }
Ejemplo n.º 11
0
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]);
}
Ejemplo n.º 12
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:
				{
				}
					
			}		
		}
	}
}
Ejemplo n.º 13
0
	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;
	}
Ejemplo n.º 14
0
	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];
			}

		}
	}
Ejemplo n.º 15
0
    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???
}
Ejemplo n.º 17
0
	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);
}
Ejemplo n.º 20
0
    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;
    }
Ejemplo n.º 21
0
    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;
    }
Ejemplo n.º 22
0
    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;
}
Ejemplo n.º 24
0
	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;
				}
			}

		}



	}