Esempio n. 1
0
void BSDE_SolidOBB_PlaneExtents(bsde_real *mins, bsde_real *maxs,
	bsde_real *xform, bsde_real *norm, bsde_real *min, bsde_real *max)
{
	bsde_real w[3];
	bsde_real m, n, d;
	int i;

	w[0]=V3_DOT(xform+(0*4), norm);
	w[1]=V3_DOT(xform+(1*4), norm);
	w[2]=V3_DOT(xform+(2*4), norm);
	d=V3_DOT(xform+3*4, norm);

	m=0;
	n=0;
	for(i=0; i<3; i++)
	{
		if(w[i]>0)
		{
			m+=mins[i]*w[i];
			n+=maxs[i]*w[i];
		}else
		{
			m+=maxs[i]*w[i];
			n+=mins[i]*w[i];
		}
	}
	*min=m+d;
	*max=n+d;
}
Esempio n. 2
0
int BSDE_SolidHull_CheckCylinderCollide(BSDE_Solid *aobj, BSDE_Solid *bobj,
	bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg)
{
	bsde_real f;
	bsde_real pt[3], pt2[3];
	bsde_real m, n;
	int i;

	f=V3_DOT(aorg, bxform+2*4)-V3_DOT(borg, bxform+2*4);

	if(f>(bobj->height*0.5))f=(bobj->height*0.5);
	if(f<(-(bobj->height*0.5)))f=-(bobj->height*0.5);

	pt[0]=borg[0]+f*bxform[2*4+0];
	pt[1]=borg[1]+f*bxform[2*4+1];
	pt[2]=borg[2]+f*bxform[2*4+2];

	BSDE_SolidHull_NearestPoint(aobj, axform, pt, pt2);
	if(V3_DIST(pt, pt2)>=bobj->radius)
		return(0);

	if(bobj->solidtype==BSDE_SOLID_CYLINDER)
	{
		f=V3_DOT(pt2, bxform+2*4)-V3_DOT(borg, bxform+2*4);
		if(f>(bobj->height*0.5))return(0);
		if(f<(-(bobj->height*0.5)))return(0);
	}

	return(1);
}
Esempio n. 3
0
void BSDE_SolidOBB_ObbNearestPoint(
	bsde_real *mins, bsde_real *maxs, bsde_real *pos,
	bsde_real *org, bsde_real *pt)
{
	bsde_real n[3], o[3];
	bsde_real d;
	int i;

	V3_COPY(org, pt);
	V3_COPY(pos+12, o);

	for(i=0; i<3; i++)
	{
		V3_COPY(pos+(i*4), n);
		d=V3_DOT(pt, n)-V3_DOT(o, n);
		if(d<mins[i])
		{
			V3_ADDSCALE(pt, n, mins[i]-d, pt);
		}
		if(d>maxs[i])
		{
			V3_ADDSCALE(pt, n, maxs[i]-d, pt);
		}
	}
}
Esempio n. 4
0
int BSDE_SolidOBB_CheckObbPlane(bsde_real *mins, bsde_real *maxs,
	bsde_real *xform, bsde_real *norm, bsde_real *io)
{
	bsde_real w[3];

	bsde_real xwa, ywa, zwa;
	bsde_real m, n;
	int i;

	w[0]=V3_DOT(xform+(0*4), norm);
	w[1]=V3_DOT(xform+(1*4), norm);
	w[2]=V3_DOT(xform+(2*4), norm);

	xwa=xform[3*4+0]*norm[0];
	ywa=xform[3*4+1]*norm[1];
	zwa=xform[3*4+2]*norm[2];

	m=0;
	n=0;
	for(i=0; i<3; i++)
	{
		if(w[i]>0)
		{
			m+=mins[i]*w[i];
			n+=maxs[i]*w[i];
		}else
		{
			m+=maxs[i]*w[i];
			n+=mins[i]*w[i];
		}
	}
	m+=xwa+ywa+zwa-norm[3];
	n+=xwa+ywa+zwa-norm[3];

	i=0;
	if(m<0)i|=2;
	if(n>=0)i|=1;

	if(!i)
	{
		printf("BSDE_SolidOBB_CheckObbPlane: fail\n");

		printf("[(%f %f %f) (%f %f %f)]\n",
			mins[0], mins[1], mins[2],
			maxs[0], maxs[1], maxs[2]);

		printf("(%f %f %f %f)\n",
			norm[0], norm[1], norm[2], norm[3]);

		printf("%f %f\n", m, n);
	}

	if(io && (i==3))
		BSDE_SolidOBB_ObbPlaneIntersect(
			mins, maxs, xform, norm, io);

	return(i);
}
Esempio n. 5
0
int BSDE_SolidOBB_ObbImpactNormal(
	bsde_real *amins, bsde_real *amaxs, bsde_real *aspos, bsde_real *aepos,
	bsde_real *bmins, bsde_real *bmaxs, bsde_real *bspos, bsde_real *bepos,
	bsde_real *vel, bsde_real *norm)
{
	bsde_real rv[3], rn[3];
	bsde_real ext[2];
	int i;

	rv[0]=V3_DOT(vel, bepos+0*4);
	rv[1]=V3_DOT(vel, bepos+1*4);
	rv[2]=V3_DOT(vel, bepos+2*4);

	if(fabs(rv[0])>fabs(rv[1]))
	{
		if(fabs(rv[0])>fabs(rv[2]))
		{
			rn[0]=(rv[0]>0)?1:-1;
			rn[1]=0;
			rn[2]=0;
		}else
		{
			rn[0]=0;
			rn[1]=0;
			rn[2]=(rv[2]>0)?1:-1;
		}
	}else
	{
		if(fabs(rv[1])>fabs(rv[2]))
		{
			rn[0]=0;
			rn[1]=(rv[1]>0)?1:-1;
			rn[2]=0;
		}else
		{
			rn[0]=0;
			rn[1]=0;
			rn[2]=(rv[2]>0)?1:-1;
		}
	}

	for(i=0; i<3; i++)
	{
		norm[i]=rn[0]*bepos[0*4+i]+
			rn[1]*bepos[1*4+i]+
			rn[2]*bepos[2*4+i];
	}

	BSDE_SolidOBB_ObbPlaneExtents(bmins, bmaxs, bepos,
		norm, ext);
	norm[3]=ext[0];	//near extent

	return(0);
}
Esempio n. 6
0
void BSDE_Mat4_InverseTransform(bsde_real *a, bsde_real *b)
{
	b[0]=a[0];	b[1]=a[4];	b[2]=a[8];	b[3]=0;
	b[4]=a[1];	b[5]=a[5];	b[6]=a[9];	b[7]=0;
	b[8]=a[2];	b[9]=a[6];	b[10]=a[10];	b[11]=0;
	b[12]=-a[12];	b[13]=-a[13];	b[14]=-a[14];	b[15]=1;

	b[12]=-V3_DOT(a+3*4, b+0*4);
	b[13]=-V3_DOT(a+3*4, b+1*4);
	b[14]=-V3_DOT(a+3*4, b+2*4);
}
Esempio n. 7
0
int BSDE_SolidSphere_CollideObbContact(
    bsde_real arad, bsde_real *aorg,
    bsde_real *bmins, bsde_real *bmaxs, bsde_real *bpos,
    bsde_real *org, bsde_real *norm, bsde_real *dist)
{
    bsde_real pt[3], dv[3];
    bsde_real ext[4];
    bsde_real bn[3];
    bsde_real d0, d1, d, bd, bo;
    int i, j;

    BSDE_SolidOBB_ObbNearestPoint(bmins, bmaxs, bpos, aorg, pt);
    V3_SUB(aorg, pt, dv);
    d=V3_NORMALIZE(dv, dv);
    if(d<0.0001)
    {
        dv[2]=1;
        pt[2]-=arad;
    }

    if(d>=arad)return(0);

    V3_COPY(dv, bn);
    bd=arad-d;

    for(i=0; i<3; i++)
    {
        ext[0]=V3_DOT(aorg, bpos+i*4)-arad;
        ext[1]=V3_DOT(aorg, bpos+i*4)+arad;
        BSDE_SolidOBB_ObbPlaneExtents(bmins, bmaxs, bpos,
                                      bpos+i*4, ext+2);
        d0=(ext[0]>ext[2])?ext[0]:ext[2];
        d1=(ext[1]<ext[3])?ext[1]:ext[3];

        d=d1-d0;
        if(d<0)return(0);

        if(d<bd)
        {
            V3_COPY(bpos+i*4, bn);
            bd=d;
        }
    }

    if(org)
    {
        V3_COPY(pt, org);
        V3_NORMALIZE(bn, bn);
        V3_COPY(bn, norm);
        *dist=bd;
    }
    return(1);
}
Esempio n. 8
0
int BSDE_SolidHull_ContactHull(BSDE_Solid *aobj, BSDE_Solid *bobj,
	bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
	bsde_real *org, bsde_real *norm, bsde_real *dist)
{
	static bsde_real anv[64*4], bnv[64*4];

	bsde_real bn[4], bo[3], tn[4], pt[3];
	bsde_real bd;
	bsde_real m, n, o, p, d, f;
	int i, j, k;

	i=BSDE_SolidHull2_CalcCollide(aobj, bobj,
		aobj->norms, bobj->norms,
		aobj->vecs, bobj->vecs, aobj->n_faces, bobj->n_faces,
		aobj->n_vecs, bobj->n_vecs,
		axform, bxform, org, norm, dist);
	return(i);

	V3_SUB(aorg, borg, bn);
	V3_NORMALIZE(bn, norm);

	BSDE_PlaneExtents(aobj, norm, &m, &n);
	BSDE_PlaneExtents(bobj, norm, &o, &p);
	m=(m>o)?m:o; n=(n<p)?n:p;

	d=V3_DOT(borg, norm);
	V3_ADDSCALE(borg, norm, ((m+n)/2)-d, org);
	*dist=n-m;
	return(1);
}
Esempio n. 9
0
void BSDE_Mat3_Normalize(bsde_real *rot)
{
	V3_NORMALIZE(rot+0, rot+0);
	V3_ADDSCALE(rot+3, rot+0, -V3_DOT(rot+3, rot+0), rot+3);
	V3_NORMALIZE(rot+3, rot+3);
	V3_CROSS(rot+0, rot+3, rot+6);
}
Esempio n. 10
0
int BSDE_SolidOBB_ObbPlaneExtents(bsde_real *mins, bsde_real *maxs,
	bsde_real *xform, bsde_real *norm, bsde_real *ext)
{
	bsde_real w[3];

	bsde_real xwa, ywa, zwa;
	bsde_real m, n;
	int i;

	w[0]=V3_DOT(xform+(0*4), norm);
	w[1]=V3_DOT(xform+(1*4), norm);
	w[2]=V3_DOT(xform+(2*4), norm);

	xwa=xform[3*4+0]*norm[0];
	ywa=xform[3*4+1]*norm[1];
	zwa=xform[3*4+2]*norm[2];

	m=0;
	n=0;

	for(i=0; i<3; i++)
	{
		if(w[i]>0)
		{
			m+=mins[i]*w[i];
			n+=maxs[i]*w[i];
		}else
		{
			m+=maxs[i]*w[i];
			n+=mins[i]*w[i];
		}
	}
	m+=xwa+ywa+zwa;
	n+=xwa+ywa+zwa;

	ext[0]=m;
	ext[1]=n;

	return(0);
}
Esempio n. 11
0
int BSDE_SolidHull2_CollideOBB(
	bsde_real *amins, bsde_real *amaxs,
	bsde_real *bmins, bsde_real *bmaxs,
	bsde_real *axform, bsde_real *bxform,
	bsde_real *rorg, bsde_real *rnorm, bsde_real *rdist)
{
	bsde_real apts[8*3], bpts[8*3];
	bsde_real ant[6*4], bnt[6*4];
	bsde_real dir[4];
	bsde_real d0, d1, d2, d3, m, n, d;
	int i;

	BSDE_SolidOBB3_GenPointsLocal(amins, amaxs, apts);
	BSDE_SolidOBB3_GenPointsLocal(bmins, bmaxs, bpts);
	BSDE_SolidOBB3_GenNormsLocal(amins, amaxs, ant);
	BSDE_SolidOBB3_GenNormsLocal(bmins, bmaxs, bnt);


	if(0)
	{
		V3_SUB(axform+3*4, bxform+3*4, dir);
		V3_NORMALIZE(dir, dir);
		dir[3]=0;

		BSDE_SolidHull2_ProjectPointsLine(apts, 8,
			axform, dir, &d0, &d1);
		BSDE_SolidHull2_ProjectPointsLine(bpts, 8,
			bxform, dir, &d2, &d3);

		m=(d0>d2)?d0:d2;
		n=(d1<d3)?d1:d3;
		if(m>n)return(0);	//somehow, no collision

		d=(m+n)/2-V3_DOT(bxform+3*4, dir);
		V3_ADDSCALE(bxform+3*4, dir, d, rorg);
		V3_COPY(dir, rnorm);
//		V3_SCALE(dir, -1, rnorm);
		*rdist=n-m;
		return(1);
	}

	i=BSDE_SolidHull2_CalcCollide(NULL, NULL,
		ant, bnt, apts, bpts, 6, 6, 8, 8, 
		axform, bxform, rorg, rnorm, rdist);

//	printf("Collide OBB %d (%g %g %g) %g\n", i,
//		rnorm[0], rnorm[1], rnorm[2], rdist);

	return(i);
}
Esempio n. 12
0
int BSDE_SolidOBB_ObbPointCollide(
	bsde_real *mins, bsde_real *maxs, bsde_real *pos, bsde_real *org)
{
	bsde_real d;
	int i;

	for(i=0; i<3; i++)
	{
		d=V3_DOT(org, pos+(i*4))-pos[3*4+i];
		if(d<mins[i])return(0);
		if(d>maxs[i])return(0);
	}
	return(1);
}
Esempio n. 13
0
int BSDE_SolidHull_ContactBox(BSDE_Solid *aobj, BSDE_Solid *bobj,
	bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
	bsde_real *org, bsde_real *norm, bsde_real *dist)
{
	static bsde_real bpts[8*4];
	static bsde_real bnv[6*4];

	bsde_real bn[4], bo[3], tn[4], pt[3];
	bsde_real bd;
	bsde_real m, n, o, p, d, f;
	int i, j, k;

	BSDE_SolidOBB3_GenPointsLocal(bobj->mins, bobj->maxs, bpts);
	BSDE_SolidOBB3_GenNormsLocal(bobj->mins, bobj->maxs, bnv);

	if(aobj->moveflags&(BSDE_MOVEFL_STATIC|BSDE_MOVEFL_SEMISTATIC))
	{
		i=BSDE_SolidHull2_CalcCollideStatic(aobj, bobj,
			aobj->norms, bnv,
			aobj->vecs, bpts, aobj->n_faces, 6,
			aobj->n_vecs, 8,
			axform, bxform, org, norm, dist);
		return(i);
	}

	i=BSDE_SolidHull2_CalcCollide(aobj, bobj,
		aobj->norms, bnv,
		aobj->vecs, bpts, aobj->n_faces, 6,
		aobj->n_vecs, 8,
		axform, bxform, org, norm, dist);
	return(i);

#if 0
	V3_SUB(aorg, borg, bn);
	V3_NORMALIZE(bn, norm);

	BSDE_PlaneExtents(aobj, norm, &m, &n);
	BSDE_PlaneExtents(bobj, norm, &o, &p);
	m=(m>o)?m:o; n=(n<p)?n:p;

	d=V3_DOT(borg, norm);
	V3_ADDSCALE(borg, norm, ((m+n)/2)-d, org);
	*dist=n-m;
	return(1);
#endif
}
Esempio n. 14
0
void BSDE_SolidHull_ProjectLine(BSDE_Solid *obj,
	bsde_real *xform, bsde_real *norm, bsde_real *min, bsde_real *max)
{
	bsde_real pt[3];
	bsde_real m, n, f;
	int i;

	m=999999999.0;
	n=-999999999.0;
	
	for(i=0; i<obj->n_vecs; i++)
	{
		BSDE_Plane_TransformPoint(obj->vecs+i*3, xform, pt);
		f=V3_DOT(pt, norm);
		if(f<m)m=f;
		if(f>n)n=f;
	}

	*min=m;
	*max=n;
}
Esempio n. 15
0
void BSDE_SolidHull2_ProjectPointsLine(bsde_real *vecs, int nvecs,
	bsde_real *xform, bsde_real *norm, bsde_real *min, bsde_real *max)
{
	bsde_real pt[3];
	bsde_real m, n, f;
	int i;

	m=999999999.0;
	n=-999999999.0;
	
	for(i=0; i<nvecs; i++)
	{
		BSDE_Plane_TransformPoint(vecs+i*3, xform, pt);
		f=V3_DOT(pt, norm);
		if(f<m)m=f;
		if(f>n)n=f;
	}

	*min=m;
	*max=n;
}
Esempio n. 16
0
void BSDE_Update(BSDE_World *world, BSDE_Solid *cur, bsde_real dt)
{
	bsde_real f;
	int i;

	f=V3_DIST(cur->e_org, cur->org)+V4_DIST(cur->e_rot, cur->rot);

//	if(f<(0.10*dt))
	if(f<(0.25*dt))
	{
		cur->idletime+=dt;
		if(cur->idletime>=1)
			cur->moveflags|=BSDE_MOVEFL_IDLE;
	}else
	{
		cur->idletime=0;
//		cur->moveflags&=~BSDE_MOVEFL_IDLE;
		cur->moveflags&=~BSDE_MOVEFL_IDLE;
		cur->stateflags&=~BSDE_STATEFL_MOVECACHE;
	}

	V3_COPY(cur->e_org, cur->org);
	V4_COPY(cur->e_rot, cur->rot);

	if(!bsde_finite(V3_DOT(cur->org, cur->org)))
		{ V3_ZERO(cur->org); }
	if(!bsde_finite(V4_DOT(cur->rot, cur->rot)))
		{ V4_ZERO(cur->rot); cur->rot[3]=1; }

	if(cur->moveflags&BSDE_MOVEFL_IDLE)
	{
		V3_ZERO(cur->tvel);
		V3_ZERO(cur->vel);
//		V3_ZERO(cur->avel);
	}
}
Esempio n. 17
0
int BSDE_SolidHull_ContactSphere(BSDE_Solid *aobj, BSDE_Solid *bobj,
	bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
	bsde_real *org, bsde_real *norm, bsde_real *dist)
{
	static bsde_real bpts[8*4];
	static bsde_real bnv[6*4];

	bsde_real bn[4], bo[3], tn[4], pt[3];
	bsde_real bd;
	bsde_real m, n, o, p, d, f;
	int i, j, k;

	if(aobj->moveflags&(BSDE_MOVEFL_STATIC|BSDE_MOVEFL_SEMISTATIC))
	{
//		printf("Sphere Static Hukk\n");

		i=BSDE_SolidHull2_CalcCollideStaticSphere(aobj, bobj,
			aobj->norms, aobj->vecs, aobj->n_faces,
			aobj->n_vecs, axform, bxform, org, norm, dist);
		return(i);
	}

#if 1
	V3_SUB(aorg, borg, bn);
	V3_NORMALIZE(bn, norm);

	BSDE_PlaneExtents(aobj, norm, &m, &n);
	BSDE_PlaneExtents(bobj, norm, &o, &p);
	m=(m>o)?m:o; n=(n<p)?n:p;

	d=V3_DOT(borg, norm);
	V3_ADDSCALE(borg, norm, ((m+n)/2)-d, org);
	*dist=n-m;
	return(1);
#endif
}
Esempio n. 18
0
void BSDE_UpdateVel(BSDE_World *world, BSDE_Solid *cur, bsde_real dt)
{
	bsde_real f;

#if 1
	if(!bsde_finite(V3_DOT(cur->ivel, cur->ivel)))
		{ V3_ZERO(cur->ivel); }
	if(!bsde_finite(V3_DOT(cur->itvel, cur->itvel)))
		{ V3_ZERO(cur->itvel); }
#endif

#if 0
	if(V3_LEN(cur->ivel)>10)
	{
		V3_SCALE(cur->ivel, 10/V3_LEN(cur->ivel), cur->ivel);
	}
	if((V3_LEN(cur->itvel)/cur->mass)>10)
	{
		V3_SCALE(cur->itvel, 10/(V3_LEN(cur->itvel)/cur->mass),
			cur->itvel);
	}
#endif


	V3_ADD(cur->vel, cur->ivel, cur->vel);
	V3_ADD(cur->tvel, cur->itvel, cur->tvel);
	V3_ZERO(cur->ivel);
	V3_ZERO(cur->itvel);

	if(V3_LEN(cur->vel)<0.01) { V3_ZERO(cur->vel); }
	if((V3_LEN(cur->tvel)/cur->mass)<0.01) { V3_ZERO(cur->tvel); }

#if 1
	if(!bsde_finite(V3_DOT(cur->vel, cur->vel)))
		{ V3_ZERO(cur->vel); }
	if(!bsde_finite(V3_DOT(cur->tvel, cur->tvel)))
		{ V3_ZERO(cur->tvel); }
#endif

#if 0
	if(V3_LEN(cur->vel)>100)
	{
		V3_SCALE(cur->vel, 100/V3_LEN(cur->vel), cur->vel);
	}

	if((V3_LEN(cur->tvel)/cur->mass)>10)
	{
		V3_SCALE(cur->tvel, 10/(V3_LEN(cur->vel)/cur->mass),
			cur->tvel);
	}
#endif

	if((cur->solidtype==BSDE_SOLID_AABB) ||
		(cur->moveflags&BSDE_MOVEFL_NOSPIN))
	{
		BSDE_Quat_Identity(cur->rot);
		V3_ZERO(cur->tvel);
	}

	if(cur->moveflags&BSDE_MOVEFL_NOMOVE)
	{
		V3_ZERO(cur->vel);
	}


//	f=V3_DIST(cur->e_org, cur->org)+V4_DIST(cur->e_rot, cur->rot);
	f=V3_LEN(cur->vel)+V3_LEN(cur->avel);
	if(f>=(0.25*dt))
	{
		cur->idletime=0;
		cur->moveflags&=~BSDE_MOVEFL_IDLE;
	}
}
Esempio n. 19
0
int BSDE_SolidHull2_CalcCollideStaticSphere(
	BSDE_Solid *aobj, BSDE_Solid *bobj,
	bsde_real *anorm, bsde_real *avecs, int anf, int anv,
	bsde_real *axform, bsde_real *bxform,
	bsde_real *org, bsde_real *norm, bsde_real *dist)
{
	static bsde_real pts0[64*4], pts1[64*4];
	bsde_real dir[4], dir1[4], dir2[4], bn[4], bo[3];
	bsde_real ldir[4], lorg[3];
	bsde_real m, n, o, p, d, bd, d0, d1, d2, d3;
	bsde_real f;
	int i, j, k;

	V4_ZERO(bn);
	bd=999999;


	for(i=0; i<anf; i++)
	{
		BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);

		f=V3_DOT(bxform+12, dir);
		m=f-bobj->radius; n=f+bobj->radius;

		if(m>dir[3])return(0);

		f=V3_NDOT(bxform+3*4, dir);
//		f=V3_NDOT(bobj->org, dir);
		if(f<0)continue;

		f=dir[3]-m;
		if(f<bd)
		{
//			V4_COPY(dir, bn);
			V4_SCALE(dir, -1, bn);
			bd=f;
		}
	}

	if(bd>999990)
	{
		printf("no plane (Sph)\n");

		BSDE_SolidHull_NearestPoint(aobj, axform, bxform+12, bo);
		if(V3_DIST(bxform+12, bo)<0.001)V3_COPY(axform+12, bo);

//		V3_SUB(borg, bo, dir);
		V3_SUB(bo, bxform+3*4, dir);
		V3_NORMALIZE(dir, dir);
//		dir[3]=V3_DOT(bo, dir);

		BSDE_PlaneExtents(aobj, dir, &m, &n);
		BSDE_PlaneExtents(bobj, dir, &o, &p);
		m=(m>o)?m:o; n=(n<p)?n:p;

		d=V3_DOT(bxform+3*4, dir);
		V3_ADDSCALE(bxform+3*4, dir, ((m+n)/2)-d, org);
		*dist=n-m;
		return(1);
	}

	V3_ADDSCALE(bxform+3*4, bn, -bobj->radius, org);
	org[3]=bd;

	V3_COPY(bn, norm);
//	V3_SCALE(bn, -1, norm);
	*dist=bd;

	return(1);
}
Esempio n. 20
0
int BSDE_SolidHull2_CalcCollide(
	BSDE_Solid *aobj, BSDE_Solid *bobj,
	bsde_real *anorm, bsde_real *bnorm,
	bsde_real *avecs, bsde_real *bvecs,
	int anf, int bnf, int anv, int bnv,
	bsde_real *axform, bsde_real *bxform,
	bsde_real *org, bsde_real *norm, bsde_real *dist)
{
	static bsde_real pts0[16*4], pts1[16*4];
	bsde_real dir[4], dir1[4], dir2[4], bn[4], bo[3];
	bsde_real ldir[4], lorg[3];
	bsde_real m, n, bd, d0, d1, d2, d3;
	bsde_real f;
	int i, j, k;

	V3_ZERO(bn);
	bd=999999;
//	bd=0;

//	printf("m0\n");

	V3_SUB(axform+3*4, bxform+3*4, ldir);
	V3_NORMALIZE(ldir, ldir);

#if 0
	BSDE_SolidHull2_ProjectPointsLine(avecs, anv, axform, ldir, &d0, &d1);
	BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv, bxform, ldir, &d2, &d3);
	m=(d0>d2)?d0:d2; n=(d1<d3)?d1:d3;
	ldir[3]=(m+n)/2;
	V4_COPY(ldir, bn); bd=ldir[3]-m;
#endif

#if 1
	for(i=0; i<anf; i++)
	{
//		printf("N (%g %g %g %g)\n",
//			anorm[i*4+0], anorm[i*4+1],
//			anorm[i*4+2], anorm[i*4+3]);

		BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);
		BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
			bxform, dir, &m, &n);
		if(m>dir[3])return(0);
//		if(n<dir[3])continue;
//		if((dir[3]-m)<0.01)continue;

//		f=(dir[3]-m)*(2.0-fabs(V3_DOT(bn, norm)));
		f=dir[3]-m;
		if(f<bd)
//		if((dir[3]-m)<bd)
		{
//			V4_COPY(dir, bn);
			V4_SCALE(dir, -1, bn);
			bd=dir[3]-m;
		}
	}

	for(i=0; i<bnf; i++)
	{
		BSDE_Plane_TransformNormal(bnorm+i*4, bxform, dir);
		BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
			axform, dir, &m, &n);
		if(m>dir[3])return(0);
//		if(n<dir[3])continue;
//		if((dir[3]-m)<0.01)continue;

//		f=(dir[3]-m)*(2.0-fabs(V3_DOT(bn, norm)));
		f=dir[3]-m;
		if(f<bd)
//		if((dir[3]-m)<bd)
		{
//			V4_SCALE(dir, -1, bn);
			V4_COPY(dir, bn);
			bd=dir[3]-m;
		}
	}

#if 0
	for(i=0; i<anf; i++)
		for(j=0; j<bnf; j++)
	{
		BSDE_Plane_TransformNormal(anorm+i*4, axform, dir1);
		BSDE_Plane_TransformNormal(bnorm+j*4, bxform, dir2);
		V3_CROSS(dir1, dir2, dir);
		bd=V3_NORMALIZE(dir, dir);
		if(bd<0.01)continue;

		BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
			axform, dir, &d0, &d1);
		BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
			bxform, dir, &d2, &d3);

		m=(d0>d2)?d0:d2;
		n=(d1<d3)?d1:d3;
		if(m>n)return(0);

		dir[3]=(m+n)/2;

		if((n-m)<bd)
		{
//			V4_COPY(dir, bn);
			V4_SCALE(dir, -1, bn);
			bd=n-m;
		}
	}
#endif
#endif

//	printf("m1\n");

	BSDE_SolidHull_MakePlaneFace(bn, pts0);
	j=4;

//	for(i=0; i<j; i++)
//		printf("\t(%g %g %g)\n", pts0[i*3+0], pts0[i*3+1], pts0[i*3+2]);

	for(i=0; i<anf; i++)
	{
		BSDE_Plane_TransformNormal(anorm+i*4, axform, dir);
		j=BSDE_SolidHull_ClipFace(dir, pts0, pts1, j);
		for(k=0; k<(j*3); k++)pts0[k]=pts1[k];
	}

	for(i=0; i<bnf; i++)
	{
		BSDE_Plane_TransformNormal(bnorm+i*4, bxform, dir);
		j=BSDE_SolidHull_ClipFace(dir, pts0, pts1, j);
		for(k=0; k<(j*3); k++)pts0[k]=pts1[k];
	}

#if 1
	if((j>0) && (bd>0.15))
	{
//		printf("A\n");

		V3_ZERO(lorg);
		for(k=0; k<j; k++) { V3_ADD(lorg, pts0+k*3, lorg); }
		V3_SCALE(lorg, 1.0/j, lorg)

		V3_SUB(axform+3*4, lorg, dir1);
//		V3_NORMALIZE(dir1, dir1);

		V3_SUB(lorg, bxform+3*4, dir2);
//		V3_NORMALIZE(dir2, dir2);

		V3_ADD(dir1, dir2, dir);

		if(aobj && bobj)
		{
			V3_SUB(bobj->vel, aobj->vel, dir1);
//			V3_SUB(aobj->vel, bobj->vel, dir1);
			V3_ADDSCALE(dir, dir1, 10, dir);
		}

		V3_NORMALIZE(dir, dir);
		dir[3]=0;

		printf("A %f %f %f\n", dir[0], dir[1], dir[2]);

		BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
			axform, dir, &d0, &d1);
		BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
			bxform, dir, &d2, &d3);

		m=(d0>d2)?d0:d2;
		n=(d1<d3)?d1:d3;
		if(m>n)return(0);	//somehow, no collision

		bd=(m+n)/2-V3_DOT(bxform+3*4, dir);
		V3_ADDSCALE(bxform+3*4, dir, bd, org);
		V3_COPY(dir, norm);
		*dist=n-m;
		return(1);
	}
#endif

#if 1
//	if(!j || (bd>0.1))
//	if(!j)
//	if(0)
//	if(bd>0.15)
//	if(bd>0.20)
//	if(bd>0.01)
	if(bd>0.25)
//	if(bd>0.10)
	{

		printf("B\n");

		V3_ZERO(dir1); V3_ZERO(dir2);
		for(i=0; i<anv; i++)
			{ V3_ADD(dir1, avecs+i*3, dir1); }
		for(i=0; i<bnv; i++)
			{ V3_ADD(dir2, bvecs+i*3, dir2); }
		V3_SCALE(dir1, 1.0/anv, dir1);
		V3_SCALE(dir2, 1.0/bnv, dir2);

		V3_SUB(dir1, dir2, dir);
//		V3_SUB(dir2, dir1, dir);

//		V3_SUB(axform+3*4, bxform+3*4, dir);
//		V3_SUB(axform+3*4, bxform+3*4, dir);

		if(aobj && bobj)
		{
			V3_SUB(bobj->vel, aobj->vel, dir1);
//			V3_SUB(aobj->vel, bobj->vel, dir1);
			V3_ADDSCALE(dir, dir1, 10, dir);
		}

		V3_NORMALIZE(dir, dir);
		dir[3]=0;

		BSDE_SolidHull2_ProjectPointsLine(avecs, anv,
			axform, dir, &d0, &d1);
		BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv,
			bxform, dir, &d2, &d3);

		m=(d0>d2)?d0:d2;
		n=(d1<d3)?d1:d3;
		if(m>n)return(0);	//somehow, no collision

		bd=(m+n)/2-V3_DOT(bxform+3*4, dir);
		V3_ADDSCALE(bxform+3*4, dir, bd, org);
		V3_COPY(dir, norm);
//		V3_SCALE(dir, -1, norm);
		*dist=n-m;
		return(1);
	}
#endif

/*
	if(!j)
	{
		printf("clipped away\n");
		return(0);
	}
*/


	for(i=0; i<j; i++)
	{
		V3_COPY(pts0+i*3, org+i*4);
		org[i*4+3]=fabs(V3_NDOT(pts0+i*3, bn));
	}
	V3_COPY(bn, norm);
//	V3_SCALE(bn, -1, norm);
	*dist=bd;

	return(j);
}
Esempio n. 21
0
static void computeCurvature(VerTex *vertex,int nvt,FaCe *face, int nfc,int* ref_tab,int nb,float* curv)
{
    int n,m,reference;
    VECTOR *v_n, *v_e1,*v_e2,*v;
    float nx,ny,nz,area,dx,dy,dz,y,r2,u1,u2,YR2,R4;

    v_n=VectorAlloc(3,MATRIX_REAL);
    v_e1=VectorAlloc(3,MATRIX_REAL);
    v_e2=VectorAlloc(3,MATRIX_REAL);
    v=VectorAlloc(3,MATRIX_REAL);
    for (n=0; n<nb; n++)
    {
        reference=ref_tab[n];
        //first need to compute normal
        nx=ny=nz=area=0;
        for (m=0; m<vertex[reference].fnum; m++)
        {
            nx+=face[vertex[reference].f[m]].nx*face[vertex[reference].f[m]].area;
            ny+=face[vertex[reference].f[m]].ny*face[vertex[reference].f[m]].area;
            nz+=face[vertex[reference].f[m]].nz*face[vertex[reference].f[m]].area;
            area+=face[vertex[reference].f[m]].area;
        }
        nx/=area;
        ny/=area;
        nz/=area;
        VECTOR_LOAD(v_n,nx,ny,nz);
        //now need to compute the tangent plane!
        VECTOR_LOAD(v,ny,nz,nx);
        V3_CROSS_PRODUCT(v_n,v,v_e1);
        if ((V3_LEN_IS_ZERO(v_e1)))
        {
            if (nz!=0)
                VECTOR_LOAD(v,ny,-nz,nx)
                else if (ny!=0)
                    VECTOR_LOAD(v,-ny,nz,nx)
                    else
                        VECTOR_LOAD(v,ny,nz,-nx);
            V3_CROSS_PRODUCT(v_n,v,v_e1);
        }
        V3_CROSS_PRODUCT(v_n,v_e1,v_e2);
        V3_NORMALIZE(v_e1,v_e1);
        V3_NORMALIZE(v_e2,v_e2);
        //finally compute curvature by fitting a 1-d quadratic r->a*r*r: curv=2*a

        for (YR2=0,R4=0,m=0; m<vertex[reference].vnum; m++)
        {
            dx=vertex[vertex[reference].v[m]].x-vertex[reference].x;
            dy=vertex[vertex[reference].v[m]].y-vertex[reference].y;
            dz=vertex[vertex[reference].v[m]].z-vertex[reference].z;
            VECTOR_LOAD(v,dx,dy,dz);

            y=V3_DOT(v,v_n);
            u1=V3_DOT(v_e1,v);
            u2=V3_DOT(v_e2,v);
            r2=u1*u1+u2*u2;
            YR2+=y*r2;
            R4+=r2*r2;
        }
        curv[n]=2*YR2/R4;
    }


    VectorFree(&v);
    VectorFree(&v_n);
    VectorFree(&v_e1);
    VectorFree(&v_e2);
}
Esempio n. 22
0
int BSDE_SolidHull_ContactGeneric(BSDE_Solid *aobj, BSDE_Solid *bobj,
	bsde_real *axform, bsde_real *bxform, bsde_real *aorg, bsde_real *borg,
	bsde_real *org, bsde_real *dir, bsde_real *dist)
{
	bsde_real norm[4], bn[4], bo[3];
	bsde_real bd;
	bsde_real m, n, o, p, d, f;
	int i;

#if 0
	if(bobj->solidtype==BSDE_SOLID_SPHERE)
	{
		BSDE_SolidHull_NearestPoint(aobj, axform, borg, bo);
		if(V3_DIST(borg, bo)<0.001)V3_COPY(aorg, bo);

//		V3_SUB(borg, bo, dir);
		V3_SUB(bo, borg, dir);
		V3_NORMALIZE(dir, dir);
//		dir[3]=V3_DOT(bo, dir);

		BSDE_PlaneExtents(aobj, dir, &m, &n);
		BSDE_PlaneExtents(bobj, dir, &o, &p);
		m=(m>o)?m:o; n=(n<p)?n:p;

		d=V3_DOT(borg, dir);
		V3_ADDSCALE(borg, dir, ((m+n)/2)-d, org);
		*dist=n-m;
		return(1);

#if 0
		V3_SUB(bo, borg, bn);
		d=bobj->radius/V3_LEN(bn);
//		V3_SCALE(bn, d, bn);
//		V3_ADD(borg, bn, org);
		V3_ADDSCALE(borg, bn, d, org);

		BSDE_PlaneExtents(aobj, dir, &m, &n);
		*dist=V3_DOT(org, dir)-n;

		return(1);
#endif
	}
#endif

	if(bobj->solidtype==BSDE_SOLID_SPHERE)
	{
		i=BSDE_SolidHull_ContactSphere(aobj, bobj,
			axform, bxform, aorg, borg,
			org, dir, dist);
		return(i);
	}

	if((bobj->solidtype==BSDE_SOLID_AABB) ||
		(bobj->solidtype==BSDE_SOLID_OBB))
	{
		i=BSDE_SolidHull_ContactBox(aobj, bobj,
			axform, bxform, aorg, borg,
			org, dir, dist);
		return(i);
	}

	V3_SUB(aorg, borg, bn);
	V3_NORMALIZE(bn, dir);

	BSDE_PlaneExtents(aobj, dir, &m, &n);
	BSDE_PlaneExtents(bobj, dir, &o, &p);
	m=(m>o)?m:o; n=(n<p)?n:p;

	d=V3_DOT(borg, dir);
	V3_ADDSCALE(borg, dir, ((m+n)/2)-d, org);
	*dist=n-m;
	return(1);

}