Real-Time Physics Simulation Forum

Code: Select all

static inline btVector3		BaryCoord(	const btVector3& a,
									  const btVector3& b,
									  const btVector3& c,
									  const btVector3& p)
{
	const btScalar	w[]={	btCross(a-p,b-p).length(),
		btCross(b-p,c-p).length(),
		btCross(c-p,a-p).length()};
	const btScalar	isum=1/(w[0]+w[1]+w[2]);
	return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
}

static inline void			
	ProjectOrigin(	const btVector3& a,
				  const btVector3& b,
				  btVector3& prj,
				  btScalar& sqd)
{
	const btVector3	d=b-a;
	const btScalar	m2=d.length2();
	if(m2>SIMD_EPSILON)
	{	
		const btScalar	t=Clamp<btScalar>(-btDot(a,d)/m2,0,1);
		const btVector3	p=a+d*t;
		const btScalar	l2=p.length2();
		if(l2<sqd)
		{
			prj=p;
			sqd=l2;
		}
	}
}

static inline void			
	ProjectOrigin(	const btVector3& a,
					  const btVector3& b,
					  const btVector3& c,
					  btVector3& prj,
					  btScalar& sqd)
{
	const btVector3&	q=btCross(b-a,c-a);
	const btScalar		m2=q.length2();
	if(m2>SIMD_EPSILON)
	{
		const btVector3	n=q/btSqrt(m2);
		const btScalar	k=btDot(a,n);
		const btScalar	k2=k*k;
		if(k2<sqd)
		{
			const btVector3	p=n*k;
			if(	(btDot(btCross(a-p,b-p),q)>0)&&
				(btDot(btCross(b-p,c-p),q)>0)&&
				(btDot(btCross(c-p,a-p),q)>0))
			{			
				prj=p;
				sqd=k2;
			}
			else
			{
				ProjectOrigin(a,b,prj,sqd);
				ProjectOrigin(b,c,prj,sqd);
				ProjectOrigin(c,a,prj,sqd);
			}
		}
	}
}