int BSDE_SolidSphere_CollideContact( bsde_real arad, bsde_real *aorg, bsde_real brad, bsde_real *borg, bsde_real *org, bsde_real *norm, bsde_real *dist) { bsde_real dv[3]; bsde_real l, f; V3_SUB(aorg, borg, dv); l=V3_NORMALIZE(dv, dv); if(l>=(arad+brad))return(0); // l=V3_DOT(dv, dv); // f=arad*arad+brad*brad; // if(l>=f)return(0); l=V3_NORMALIZE(dv, dv); if(org) { f=(brad+(l-arad))*0.5; if(f<0)f=0; V3_ADDSCALE(borg, dv, f, org); V3_COPY(dv, norm); *dist=(arad+brad)-l; } return(1); }
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); }
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); }
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); }
int BSDE_SolidSphere_CollideObbContact( bsde_real arad, bsde_real *aorg, bsde_real *bmins, bsde_real *bmaxs, bsde_real *pos, bsde_real *org, bsde_real *norm, bsde_real *dist) { bsde_real pt[3], dv[3]; bsde_real d; BSDE_SolidOBB_ObbNearestPoint(bmins, bmaxs, pos, 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); if(org) { V3_COPY(pt, org); V3_COPY(dv, norm); *dist=arad-d; } return(1); }
void BSDE_SolidHull_ModifyHullFaces(BSDE_Solid *obj, bsde_real *norms, int n) { bsde_real d; int i; if(obj->vecs) { free(obj->vecs); obj->vecs=NULL; } if(obj->faces) { free(obj->faces); obj->faces=NULL; } if(obj->facevecs) { free(obj->facevecs); obj->facevecs=NULL; } if(obj->norms)free(obj->norms); obj->norms=(bsde_real *)malloc(4*n*sizeof(bsde_real)); obj->n_faces=n; for(i=0; i<n; i++) { V3_NORMALIZE(norms+i*4, obj->norms+i*4); obj->norms[i*4+3]=norms[i*4+3]; } if((obj->mass>0) && (obj->volume>0)) d=obj->mass/obj->volume; else d=1; BSDE_SolidHull_MakeHullFaces(obj); obj->mass=d*obj->volume; BSDE_BuildInertiaTensor(obj); }
void BSDE_Predict(BSDE_World *world, BSDE_Solid *cur, bsde_real dt) { bsde_real qa[4], qb[4], n[3], l; bsde_real ma[9], mb[9], mc[9]; int i; if(cur->stateflags&BSDE_STATEFL_MOVECACHE) return; // if(cur->moveflags&BSDE_MOVEFL_IDLE) // return; //predict origin and rotation V3_ADDSCALE(cur->org, cur->vel, dt, cur->e_org); l=V3_NORMALIZE(cur->avel, n); // printf("spin (%f %f %f) %f\n", n[0], n[1], n[2], l); BSDE_Quat_FromAxis(n, l*dt, qa); BSDE_Quat_Multiply(qa, cur->rot, qb); // BSDE_Quat_Multiply(cur->rot, qa, qb); BSDE_Quat_Normalize(qb, cur->e_rot); V3_COPY(cur->vel, cur->e_vel); V3_COPY(cur->tvel, cur->e_tvel); //calculate current inertia tensor BSDE_WorldInertiaTensor(cur); //calculate current transform BSDE_Quat_ToMatrix(cur->rot, cur->xform); for(i=0; i<3; i++)cur->xform[3*4+i]+=cur->org[i]; //calculate predicted transform BSDE_Quat_ToMatrix(cur->e_rot, cur->e_xform); for(i=0; i<3; i++)cur->e_xform[3*4+i]+=cur->e_org[i]; //calculate move volume BSDE_BoxMove(cur, cur->mmins, cur->mmaxs); //clear prediction flag (used for expensive internal predictions) cur->moveflags&=~BSDE_MOVEFL_PREDICT; if(cur->moveflags&(BSDE_MOVEFL_STATIC|BSDE_MOVEFL_IDLE)) cur->stateflags|=BSDE_STATEFL_MOVECACHE; }
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); }
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 }
void BSDE_PredictPosTime(BSDE_Solid *cur, bsde_real dt, bsde_real *org, bsde_real *rot, bsde_real *xform) { bsde_real qa[4], qb[4], n[3], l; int i; if(dt>0) { V3_ADDSCALE(cur->org, cur->vel, dt, org); l=V3_NORMALIZE(cur->avel, n); BSDE_Quat_FromAxis(n, l*dt, qa); BSDE_Quat_Multiply(qa, cur->rot, qb); BSDE_Quat_Normalize(qb, rot); }else { V3_COPY(cur->org, org); V3_COPY(cur->rot, rot); } BSDE_Quat_ToMatrix(rot, xform); for(i=0; i<3; i++)xform[3*4+i]+=org[i]; }
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 }
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); }
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); }
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); }
void BSDE_SolidHull_MakeHullFaces(BSDE_Solid *obj) { static bsde_real vecbuf[64*64*3]; static bsde_real pointbuf[64*3], pointbuf2[64*3]; static int facebuf[64*64]; static int facevbuf[64]; int i, j, k, l, n, nf, nv; int p0, p1, p2; bsde_real *v0, *v1, *v2; bsde_real dv0[3], dv1[3], dv2[3]; bsde_real f, g, h; n=0; nv=0; nf=0; // printf("build"); for(i=0; i<obj->n_faces; i++) { BSDE_SolidHull_MakePlaneFace(obj->norms+(i*4), pointbuf); l=4; for(j=0; j<obj->n_faces; j++) { if(i==j)continue; l=BSDE_SolidHull_ClipFace(obj->norms+(j*4), pointbuf, pointbuf2, l); for(k=0; k<(l*3); k++)pointbuf[k]=pointbuf2[k]; } if(!l)continue; // printf("%d:%d", i, l); for(j=0; j<l; j++) { for(k=0; k<nv; k++) if(V3_DIST(pointbuf+j*3, vecbuf+k*3)<0.00001) break; if(k==nv) // if(1) { V3_COPY(pointbuf+j*3, vecbuf+k*3); nv++; } facebuf[n++]=k; } facevbuf[nf]=l; nf++; } // printf("\n"); // printf("facevecs"); // for(i=0; i<nf; i++)printf(" %d:%d", i, facevbuf[i]); // printf("\n"); if(nf<obj->n_faces) { printf("MakeHullFaces: %d faces clipped away\n", obj->faces-nf); } // obj->n_faces=nf; obj->n_vecs=nv; obj->vecs=(bsde_real *)malloc(nv*3*sizeof(bsde_real)); memcpy(obj->vecs, vecbuf, nv*3*sizeof(bsde_real)); obj->faces=(int *)malloc(n*sizeof(int)); memcpy(obj->faces, facebuf, n*sizeof(int)); obj->facevecs=(int *)malloc(nf*sizeof(int)); memcpy(obj->facevecs, facevbuf, nf*sizeof(int)); k=0; h=0; for(i=0; i<nf; i++) { l=facevbuf[i]; for(j=1; j<(l-1); j++) { p0=facebuf[k]; p1=facebuf[k+j]; p2=facebuf[k+j+1]; if((p0==p1) || (p1==p2) || (p2==p0))continue; v0=vecbuf+p0*3; v1=vecbuf+p1*3; v2=vecbuf+p2*3; V3_SUB(v1, v0, dv0); V3_SUB(v2, v0, dv1); V3_CROSS(dv0, dv1, dv2); f=V3_NORMALIZE(dv2, dv2); g=f*0.5; V3_CROSS(dv0, v0, dv2); f=V3_NORMALIZE(dv2, dv2); g*=f*0.5; V3_CROSS(dv1, v0, dv2); f=V3_NORMALIZE(dv2, dv2); g*=f*0.5; h+=g; } k+=l; } obj->volume=h; V3_SET(obj->mins, 999999, 999999, 999999); V3_SET(obj->maxs, -999999, -999999, -999999); for(i=0; i<nv; i++) { if(vecbuf[i*3+0]<obj->mins[0])obj->mins[0]=vecbuf[i*3+0]; if(vecbuf[i*3+0]>obj->maxs[0])obj->maxs[0]=vecbuf[i*3+0]; if(vecbuf[i*3+1]<obj->mins[1])obj->mins[1]=vecbuf[i*3+1]; if(vecbuf[i*3+1]>obj->maxs[1])obj->maxs[1]=vecbuf[i*3+1]; if(vecbuf[i*3+2]<obj->mins[2])obj->mins[2]=vecbuf[i*3+2]; if(vecbuf[i*3+2]>obj->maxs[2])obj->maxs[2]=vecbuf[i*3+2]; } f=0; for(i=0; i<nv; i++) { g=V3_LEN(vecbuf+i*3); if(g>f)f=g; } obj->radius=f; obj->stateflags&=~BSDE_STATEFL_BBOX; if(obj->n_faces==6) { j=0; for(i=0; i<obj->n_faces; i++) { v0=obj->norms+i*4; if((v0[0]==1) && (v0[1]==0) && (v0[2]==0))j|=1; if((v0[0]==-1) && (v0[1]==0) && (v0[2]==0))j|=2; if((v0[0]==0) && (v0[1]==1) && (v0[2]==0))j|=4; if((v0[0]==0) && (v0[1]==-1) && (v0[2]==0))j|=8; if((v0[0]==0) && (v0[1]==0) && (v0[2]==1))j|=16; if((v0[0]==0) && (v0[1]==0) && (v0[2]==-1))j|=32; } if(j==63)obj->stateflags|=BSDE_STATEFL_BBOX; } }
int BSDE_SolidHull2_CalcCollideStatic( 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[64*4], pts1[64*4]; bsde_real dir[4], dir1[4], dir2[4], bn[4], bo[4]; bsde_real ldir[4], lorg[4]; bsde_real m, n, bd, d0, d1, d2, d3; bsde_real f; int i, j, k; V4_ZERO(bn); bd=999999; // V3_SCALE(bobj->vel, -1, bn); // V3_COPY(bobj->vel, bn); // V3_NORMALIZE(bn, bn); // bd=5; // bd=999999; // BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv, // bxform, bn, &m, &n); // bn[3]=0; 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=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\n"); // V3_SCALE(bobj->vel, -1, bn); // V3_COPY(bobj->vel, bn); V3_SUB(bobj->org, bobj->e_org, bn); V3_NORMALIZE(bn, bn); printf("Fake %f %f %f\n", bn[0], bn[1], bn[2]); BSDE_SolidHull2_ProjectPointsLine(avecs, anv, axform, bn, &d0, &d1); BSDE_SolidHull2_ProjectPointsLine(bvecs, bnv, bxform, bn, &d2, &d3); m=(d0>d2)?d0:d2; n=(d1<d3)?d1:d3; bd=n-m; bn[3]=n; f=V3_NDOT(bobj->org, bn); V3_ADDSCALE(bobj->org, bn, -f-bd, org); org[3]=bd; V3_COPY(bn, norm); *dist=bd; return(1); } 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]); #if 0 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]; } #endif 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(!j) { printf("clipped away\n"); f=V3_NDOT(bobj->org, bn); V3_ADDSCALE(bobj->org, bn, -f, org); org[3]=bd; V3_COPY(bn, norm); // V3_SCALE(bn, -1, norm); *dist=bd; return(1); 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)); org[i*4+3]=bd; } V3_COPY(bn, norm); // V3_SCALE(bn, -1, norm); *dist=bd; return(j); }
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); }