533 lines
23 KiB
C++
533 lines
23 KiB
C++
#include "stdafx.h"
|
|
|
|
#include "utils.h"
|
|
#include "primitives.h"
|
|
#include "unprojectionchecks.h"
|
|
|
|
#define UPDATE_IDBEST(tbest,newid) \
|
|
(idbest&=~-bBest) |= (newid)&-bBest; \
|
|
(tbest.x*=bBest^1) += tsin.x*bBest; \
|
|
(tbest.y*=bBest^1) += tsin.y*bBest
|
|
|
|
|
|
int tri_tri_rot_unprojection(unprojection_mode *pmode, const triangle *ptri1,int iFeature1, const triangle *ptri2,int iFeature2,
|
|
contact *pcontact, geom_contact_area *parea)
|
|
{
|
|
const triangle *ptri[2] = { ptri1,ptri2 };
|
|
vectorr pt,ptz[2][3],ptx[2][3],pty[2][3],rotax,pt0,pt1,edge0,edge1,n0z,n0x,n0y,ptz0,ptx0,pty0,n,pt0_rot,
|
|
pt1_rot,edge0_rot,ptz1,ptx1,pty1,dir0,dir1,norm[2];
|
|
quotient tsin,tcos,tmin(pmode->tmax,1),t0,t1;
|
|
real kcos,ksin,k,a,b,c,d,sg0,sg1;
|
|
int itri,i,j,j1,bContact,bBest,idbest=-1,imask=iFeature2<<8 | iFeature1;
|
|
|
|
// compute components for vec.rot(t) = vecz + vecx*cos(t) + vecy*sin(t)
|
|
for(itri=0,rotax=pmode->dir;itri<2;itri++,rotax*=-1) for(i=0;i<3;i++) {
|
|
pt = ptri[itri]->pt[i]-pmode->center;
|
|
ptz[itri][i] = rotax*(pt*rotax); ptx[itri][i] = pt-ptz[itri][i]; pty[itri][i] = rotax^ptx[itri][i];
|
|
}
|
|
|
|
// vertex-face contacts, (p[i].rot(t)-origin)*n = 0; -t for the 2nd triangle
|
|
for(itri=0;itri<2;itri++) for(i=0;i<3;i++) if ((0x40<<(itri<<3) | (0x80|i)<<((itri^1)<<3))!=imask) {
|
|
kcos = ptx[itri^1][i]*ptri[itri]->n; ksin = pty[itri^1][i]*ptri[itri]->n;
|
|
k = (ptz[itri^1][i]-ptri[itri]->pt[0]+pmode->center)*ptri[itri]->n;
|
|
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
|
|
if (d>=0) {
|
|
d = sqrt_tpl(d); tsin.set(-b-d,a);
|
|
for(j=0;j<2;j++,tsin.x+=d*2)
|
|
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
|
|
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) (=eliminate foreign root)
|
|
{
|
|
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
|
|
pt = ptx[itri^1][i]*tcos.x + pty[itri^1][i]*tsin.x + (ptz[itri^1][i]+pmode->center)*tsin.y;
|
|
bContact = // point is inside itri
|
|
isneg((pt-ptri[itri]->pt[0]*tsin.y ^ ptri[itri]->pt[1]-ptri[itri]->pt[0])*ptri[itri]->n) &
|
|
isneg((pt-ptri[itri]->pt[1]*tsin.y ^ ptri[itri]->pt[2]-ptri[itri]->pt[1])*ptri[itri]->n) &
|
|
isneg((pt-ptri[itri]->pt[2]*tsin.y ^ ptri[itri]->pt[0]-ptri[itri]->pt[2])*ptri[itri]->n);
|
|
// additionally check that triangles don't intersect during contact
|
|
pt = ptx[itri^1][inc_mod3[i]]*tcos.x + pty[itri^1][inc_mod3[i]]*tsin.x + (ptz[itri^1][inc_mod3[i]] +
|
|
pmode->center - ptri[itri]->pt[0])*tsin.y;
|
|
sg0 = pt*ptri[itri]->n + pmode->minPtDist*tsin.y;
|
|
pt = ptx[itri^1][dec_mod3[i]]*tcos.x + pty[itri^1][dec_mod3[i]]*tsin.x + (ptz[itri^1][dec_mod3[i]] +
|
|
pmode->center - ptri[itri]->pt[0])*tsin.y;
|
|
sg1 = pt*ptri[itri]->n + pmode->minPtDist*tsin.y;
|
|
bContact &= isnonneg(sg0*sg1);
|
|
bBest = bContact & isneg(tsin-tmin);
|
|
UPDATE_IDBEST(tmin,itri<<2 | i);
|
|
}
|
|
}
|
|
} else { // check if triangles are already separated in their initial state
|
|
sg0 = (ptri[itri^1]->pt[inc_mod3[i]]-ptri[itri]->pt[0])*ptri[itri]->n + pmode->minPtDist;
|
|
sg1 = (ptri[itri^1]->pt[dec_mod3[i]]-ptri[itri]->pt[0])*ptri[itri]->n + pmode->minPtDist;
|
|
bBest = isnonneg(sg0) & isnonneg(sg1); tsin.set(0,1);
|
|
UPDATE_IDBEST(tmin,itri<<2 | i);
|
|
}
|
|
|
|
rotax = pmode->dir;
|
|
n0z = rotax*(ptri[0]->n*rotax); n0x = ptri[0]->n-n0z; n0y = rotax^n0x;
|
|
// edge-edge contacts
|
|
// (p0.rot(t)-p1)*(l0.rot(t)^l1) = 0
|
|
// p0.rot(t)*(l0.rot(t)^l1) - p1*(l0.rot(t)^l1) = 0;
|
|
// l1*(p0^l0).rot(t) + l0.rot(t)*(p1^l1) = 0;
|
|
// l1*(p0^l0).rot(t) + l0*(p1^l1).rot(-t) = 0; - this one's just to get symmetrical form
|
|
// note that p.rot(t)^l.rot(t)==(p^l).rot(t) only if p is relative to the center of rotation
|
|
for(i=0;i<3;i++) {
|
|
pt0 = ptri[0]->pt[i]-pmode->center;
|
|
edge0 = ptri[0]->pt[inc_mod3[i]]-ptri[0]->pt[i];
|
|
pt = pt0^edge0; ptz0 = rotax*(pt*rotax); ptx0 = pt-ptz0; pty0 = rotax^ptx0;
|
|
for(j=0;j<3;j++) if ((0xA0|i | (0xA0|j)<<8)!=imask) {
|
|
pt1 = ptri[1]->pt[j]-pmode->center;
|
|
edge1 = ptri[1]->pt[inc_mod3[j]]-ptri[1]->pt[j];
|
|
pt = pt1^edge1; ptz1 = rotax*(pt*rotax); ptx1 = pt-ptz1; pty1 = rotax^ptx1;
|
|
kcos = edge1*ptx0+edge0*ptx1; ksin = edge1*pty0-edge0*pty1; k = edge1*ptz0+edge0*ptz1;
|
|
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
|
|
if (d>=0) {
|
|
d = sqrt_tpl(d); tsin.set(-b-d,a);
|
|
for(j1=0;j1<2;j1++,tsin.x+=d*2)
|
|
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
|
|
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) (=eliminate foreign root)
|
|
{
|
|
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
|
|
pt0_rot = ptx[0][i]*tcos.x + pty[0][i]*tsin.x + ptz[0][i]*tsin.y;
|
|
pt1_rot = ptx[0][inc_mod3[i]]*tcos.x + pty[0][inc_mod3[i]]*tsin.x + ptz[0][inc_mod3[i]]*tsin.y;
|
|
edge0_rot = pt1_rot-pt0_rot;
|
|
n = edge0_rot^edge1; pt = pt1*tsin.y-pt0_rot;
|
|
t0.set((pt^edge1)*n, n.len2());
|
|
t1.set((pt^edge0_rot)*n, t0.y*tsin.y);
|
|
bContact = isneg(fabs_tpl(t0.x*2-t0.y)-t0.y) & isneg(fabs_tpl(t1.x*2-t1.y)-t1.y);
|
|
// additionally check that triangles don't intersect during contact
|
|
dir0 = (n0z*tsin.y+n0x*tcos.x+n0y*tsin.x)^edge0_rot;
|
|
dir1 = ptri[1]->n^edge1;
|
|
bContact &= isneg((dir0*n)*(dir1*n));
|
|
bBest = bContact & isneg(tsin-tmin);
|
|
UPDATE_IDBEST(tmin,i<<2 | j | 0x80);
|
|
}
|
|
}
|
|
} else { // check if triangles are already separated in their initial state
|
|
edge1 = ptri[1]->pt[inc_mod3[j]]-ptri[1]->pt[j]; n = edge0^edge1;
|
|
dir0 = ptri[0]->n^edge0; dir1 = ptri[1]->n^edge1;
|
|
bBest = isneg((dir0*n)*(dir1*n)); tsin.set(0,1);
|
|
// additionally check that edges intersection point will not move inside the 2nd triangle during unprojection
|
|
bBest &= isneg(dir1*(pmode->dir ^ (ptri[0]->pt[i]-pmode->center)*n.len2() + edge0*((ptri[1]->pt[j]-ptri[0]->pt[i]^edge1)*n)));
|
|
UPDATE_IDBEST(tmin,i<<2 | j | 0x80);
|
|
}
|
|
}
|
|
|
|
if (idbest==-1)
|
|
return 0;
|
|
|
|
if (idbest & 0x80) {
|
|
i = idbest>>2 & 3; j = idbest & 3;
|
|
pcontact->t = tmin.val();
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pt0_rot = ptx[0][i]*pcontact->taux + pty[0][i]*pcontact->t + ptz[0][i] + pmode->center;
|
|
pt1_rot = ptx[0][inc_mod3[i]]*pcontact->taux + pty[0][inc_mod3[i]]*pcontact->t + ptz[0][inc_mod3[i]] + pmode->center;
|
|
edge0_rot = pt1_rot-pt0_rot;
|
|
edge1 = ptri[1]->pt[inc_mod3[j]]-ptri[1]->pt[j];
|
|
pt = ptri[1]->pt[j]-pt0_rot; n = edge0_rot ^ edge1;
|
|
t0.set((pt^edge1)*n, n.len2());
|
|
pcontact->pt = pt0_rot + edge0_rot*t0.val();
|
|
pcontact->n = n*sgnnz((ptri2->n^edge1)*n);
|
|
pcontact->iFeature[0] = 0xA0 | i;
|
|
pcontact->iFeature[1] = 0xA0 | j;
|
|
} else {
|
|
itri = idbest>>2; i = idbest & 3;
|
|
pcontact->t = tmin.val();
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pcontact->pt = ptz[itri^1][i] + ptx[itri^1][i]*max(pcontact->taux,(real)(itri^1)) + pty[itri^1][i]*(pcontact->t*itri) + pmode->center;
|
|
norm[0] = n0z+n0x*pcontact->taux+n0y*pcontact->t; norm[1] = -ptri[1]->n;
|
|
pcontact->n = norm[itri];
|
|
pcontact->iFeature[itri] = 0x40;
|
|
pcontact->iFeature[itri^1] = 0x80 | i;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
int ray_tri_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1, const triangle *ptri,int iFeature2,
|
|
contact *pcontact, geom_contact_area *parea)
|
|
{
|
|
vectorr pt,ptz[2],ptx[2],pty[2],rotax=pmode->dir,edge1,ptz0,ptx0,pty0,pt1,pt0_rot,pt1_rot,edge0_rot,ptz1,ptx1,pty1,n;
|
|
quotient tsin,tcos,tmin(pmode->tmax,1),t0,t1;
|
|
real kcos,ksin,k,a,b,c,d;
|
|
int i,j,bContact,bBest,idbest=-1,imask=iFeature2<<8 | iFeature1;
|
|
|
|
pt = pray->origin-pmode->center;
|
|
ptz[0] = rotax*(pt*rotax); ptx[0] = pt-ptz[0]; pty[0] = rotax^ptx[0];
|
|
pt += pray->dir;
|
|
ptz[1] = rotax*(pt*rotax); ptx[1] = pt-ptz[1]; pty[1] = rotax^ptx[1];
|
|
|
|
// ray end - triangle face
|
|
for(i=1;i>=0;i--) {
|
|
kcos = ptx[i]*ptri->n; ksin = pty[i]*ptri->n;
|
|
k = (ptz[i]-ptri->pt[0]+pmode->center)*ptri->n;
|
|
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
|
|
if (d>=0) {
|
|
d = sqrt_tpl(d); tsin.set(-b-d,a);
|
|
for(j=0;j<2;j++,tsin.x+=d*2)
|
|
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
|
|
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1)
|
|
{
|
|
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
|
|
pt = ptx[i]*tcos.x + pty[i]*tsin.x + (ptz[i]+pmode->center)*tsin.y;
|
|
bContact = // point is inside itri
|
|
isneg((pt-ptri->pt[0]*tsin.y ^ ptri->pt[1]-ptri->pt[0])*ptri->n) &
|
|
isneg((pt-ptri->pt[1]*tsin.y ^ ptri->pt[2]-ptri->pt[1])*ptri->n) &
|
|
isneg((pt-ptri->pt[2]*tsin.y ^ ptri->pt[0]-ptri->pt[2])*ptri->n);
|
|
bBest = bContact & isneg(tsin-tmin);
|
|
UPDATE_IDBEST(tmin,i);
|
|
}
|
|
}
|
|
if ((pray->origin-pmode->center).len2()<sqr(pmode->minPtDist))
|
|
break;
|
|
}
|
|
|
|
if (idbest>=0) {
|
|
i = idbest & 1;
|
|
pcontact->t = tmin.val();
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pcontact->pt = ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + pmode->center;
|
|
pcontact->n = -ptri->n;
|
|
pcontact->iFeature[0] = 0x80 | i;
|
|
pcontact->iFeature[1] = 0x40;
|
|
return 1;
|
|
}
|
|
|
|
// ray - triangle edge
|
|
pt = pray->origin-pmode->center^pray->dir; ptz0 = rotax*(pt*rotax); ptx0 = pt-ptz0; pty0 = rotax^ptx0;
|
|
for(i=0;i<3;i++) if ((0xA0 | (0xA0|i)<<8)!=imask) {
|
|
pt1 = ptri->pt[i]-pmode->center; edge1 = ptri->pt[inc_mod3[i]]-ptri->pt[i];
|
|
pt = pt1^edge1; ptz1 = rotax*(pt*rotax); ptx1 = pt-ptz1; pty1 = rotax^ptx1;
|
|
kcos = edge1*ptx0+pray->dir*ptx1; ksin = edge1*pty0-pray->dir*pty1; k = edge1*ptz0+pray->dir*ptz1;
|
|
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
|
|
if (d>=0) {
|
|
d = sqrt_tpl(d); tsin.set(-b-d,a);
|
|
for(j=0;j<2;j++,tsin.x+=d*2)
|
|
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
|
|
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1)
|
|
{
|
|
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
|
|
pt0_rot = ptx[0]*tcos.x + pty[0]*tsin.x + ptz[0]*tsin.y;
|
|
pt1_rot = ptx[1]*tcos.x + pty[1]*tsin.x + ptz[1]*tsin.y;
|
|
edge0_rot = pt1_rot-pt0_rot;
|
|
n = edge0_rot^edge1; pt = pt1*tsin.y-pt0_rot;
|
|
t0.set((pt^edge1)*n, n.len2());
|
|
t1.set((pt^edge0_rot)*n, t0.y*tsin.y);
|
|
bContact = isneg(fabs_tpl(t0.x*2-t0.y)-t0.y) & isneg(fabs_tpl(t1.x*2-t1.y)-t1.y);
|
|
bBest = bContact & isneg(tsin-tmin);
|
|
UPDATE_IDBEST(tmin,i | 0x80);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (idbest<0)
|
|
return 0;
|
|
|
|
i = idbest & 3;
|
|
pcontact->t = tmin.val();
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pt0_rot = ptx[0]*pcontact->taux + pty[0]*pcontact->t + ptz[0] + pmode->center;
|
|
pt1_rot = ptx[1]*pcontact->taux + pty[1]*pcontact->t + ptz[1] + pmode->center;
|
|
edge0_rot = pt1_rot-pt0_rot;
|
|
edge1 = ptri->pt[inc_mod3[i]]-ptri->pt[i];
|
|
pt = ptri->pt[i]-pt0_rot; n = edge0_rot ^ edge1;
|
|
t0.set((pt^edge1)*n, n.len2());
|
|
pcontact->pt = pt0_rot + edge0_rot*t0.val();
|
|
pcontact->n = n*sgnnz((ptri->n^edge1)*n);
|
|
pcontact->iFeature[0] = 0xA0;
|
|
pcontact->iFeature[1] = 0xA0 | i;
|
|
return 1;
|
|
}
|
|
|
|
int tri_ray_rot_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1, const ray *pray,int iFeature2,
|
|
contact *pcontact, geom_contact_area *parea)
|
|
{
|
|
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
|
|
int res = ray_tri_rot_unprojection(pmode,pray,iFeature2,ptri,iFeature1,pcontact,parea);
|
|
if (res) {
|
|
pcontact->pt = pcontact->pt.rotated(pmode->center,pmode->dir, pcontact->taux,-pcontact->t);
|
|
pcontact->n = pcontact->n.rotated(pmode->dir, pcontact->taux,-pcontact->t).Flip();
|
|
int iFeature = pcontact->iFeature[0]; pcontact->iFeature[0]=pcontact->iFeature[1]; pcontact->iFeature[1]=iFeature;
|
|
}
|
|
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
|
|
return res;
|
|
}
|
|
|
|
|
|
int ray_cyl_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1, const cylinder *pcyl,int iFeature2,
|
|
contact *pcontact, geom_contact_area *parea)
|
|
{
|
|
vectorr center,ccap,v,vx,vy,vz,l,lx,ly,lz,vsin,vcos,pt,ptz[2],ptx[2],pty[2], rotax=pmode->dir, axis=pcyl->axis;
|
|
real a,b,c,d,k,ksin,kcos, hh=pcyl->hh, r2=sqr(pcyl->r), len2=pray->dir.len2(), roots[4];
|
|
quotient tmax(0,1),tsin,tcos,t;
|
|
int i,j,icap,idbest=-1,bContact,bBest;
|
|
polynomial_tpl<real,4> pn;
|
|
center = pcyl->center-pmode->center;
|
|
|
|
pt = pray->origin-pmode->center;
|
|
ptz[0] = rotax*(pt*rotax); ptx[0] = pt-ptz[0]; pty[0] = rotax^ptx[0];
|
|
pt += pray->dir;
|
|
ptz[1] = rotax*(pt*rotax); ptx[1] = pt-ptz[1]; pty[1] = rotax^ptx[1];
|
|
|
|
for(i=1; i>=0; i--) {
|
|
// ray end - cylinder cap
|
|
kcos = ptx[i]*axis; ksin = pty[i]*axis;
|
|
for(icap=-1; icap<=1; icap+=2) {
|
|
k = (ptz[i]-center-axis*(hh*icap))*axis;
|
|
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
|
|
if (d>=0) {
|
|
d = sqrt_tpl(d); tsin.set(-b-d,a);
|
|
for(j=0;j<2;j++,tsin.x+=d*2)
|
|
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
|
|
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) = remove phantom roots
|
|
{
|
|
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
|
|
pt = ptx[i]*tcos.x + pty[i]*tsin.x + ptz[i]*tsin.y;
|
|
bContact = isneg((pt-(center+axis*(hh*icap))*tsin.y).len2() - r2*sqr(tsin.y));
|
|
bBest = bContact & isneg(tmax-tsin);
|
|
UPDATE_IDBEST(tmax,icap+1|i);
|
|
}
|
|
}
|
|
}
|
|
|
|
// ray end - cylinder side
|
|
v = ptz[i]-center ^ axis;
|
|
vcos = ptx[i]^axis; vsin = pty[i]^axis;
|
|
pn = psqr(P2(sqr(vsin)-sqr(vcos)) + P1((v*vsin)*2) + sqr(v)+sqr(vcos)-r2) - psqr(P1(vcos*vsin)+v*vcos)*((real)1-P2(1))*4;
|
|
if (pn.nroots(0,1)) for(j=pn.findroots(0,1,roots)-1;j>=0;j--) {
|
|
tsin.set(roots[j],1);
|
|
pt = ptz[i]+ptx[i]*sqrt_tpl(1-sqr(tsin.x))+pty[i]*tsin.x-center;
|
|
bContact = isneg(fabs_tpl(pt*axis)-hh) & inrange((pt^axis).len2(), r2*sqr((real)0.99),r2*sqr((real)1.01));
|
|
bBest = isneg(tmax-tsin) & bContact;
|
|
UPDATE_IDBEST(tmax,0x10|i);
|
|
}
|
|
|
|
if ((pray->origin-pmode->center).len2()<sqr(pmode->minPtDist))
|
|
break;
|
|
}
|
|
|
|
// ray - cylinder side
|
|
lz = rotax*(pray->dir*rotax); lx = pray->dir-lz; ly = rotax^lx;
|
|
v = pray->origin-pmode->center^pray->dir;
|
|
vz = rotax*(v*rotax); vx = v-vz; vy = rotax^vx;
|
|
v = axis^center;
|
|
kcos = axis*vx-v*lx; ksin = axis*vy-v*ly; k = axis*vz-v*lz;
|
|
vcos = lx^axis; vsin = ly^axis; v = lz^axis;
|
|
pn = psqr(P2(sqr(ksin)-sqr(kcos)-r2*(sqr(vsin)-sqr(vcos)))+P1(((ksin*k)-r2*(vsin*v))*2)+sqr(kcos)+sqr(k)-r2*(sqr(vcos)+sqr(v))) -
|
|
psqr(P1((kcos*ksin-r2*(vcos*vsin))*2)+(kcos*k-r2*(vcos*v))*2);
|
|
if (pn.nroots(0,1)) for(j=pn.findroots(0,1,roots)-1;j>=0;j--) {
|
|
tsin.set(roots[j],1); tcos.x = sqrt_tpl(1-sqr(tsin.x));
|
|
pt = ptz[0] + ptx[0]*tcos.x + pty[0]*tsin.x - center;
|
|
l = lz + lx*tcos.x + ly*tsin.x; v = l^axis; k = v.len2();
|
|
bContact = inrange(sqr(pt*v), r2*k*sqr((real)0.99),r2*k*sqr((real)1.01)); // remove phantom roots
|
|
t.set((-pt^axis)*v, k);
|
|
(pt*=t.y) += l*t.x;
|
|
bContact &= inrange(sqr(t.x), (real)0,len2*sqr(t.y));
|
|
bContact &= isneg(fabs_tpl(pt*axis)-hh*t.y);
|
|
bBest = isneg(tmax-tsin) & bContact;
|
|
UPDATE_IDBEST(tmax,0x20);
|
|
}
|
|
|
|
// ray - cylinder cap
|
|
for(icap=-1; icap<=1; icap+=2) {
|
|
ccap = center+axis*(hh*icap);
|
|
vcos = (axis^vx)-(axis^(ccap^lx)); vsin = (axis^vy)-(axis^(ccap^ly)); v = (axis^vz)-(axis^(ccap^lz));
|
|
kcos = lx*axis; ksin = ly*axis; k = lz*axis;
|
|
pn = psqr(P2(sqr(vsin)-sqr(vcos)-r2*(sqr(ksin)-sqr(kcos)))+P1(((vsin*v)-r2*(ksin*k))*2)+sqr(vcos)+sqr(v)-r2*(sqr(kcos)+sqr(k))) -
|
|
psqr(P1((vcos*vsin-r2*(kcos*ksin))*2)+(vcos*v-r2*(kcos*k))*2);
|
|
if (pn.nroots(0,1)) for(j=pn.findroots(0,1,roots)-1;j>=0;j--) {
|
|
tsin.set(roots[j],1); tcos.x = sqrt_tpl(1-sqr(tsin.x));
|
|
pt = ptz[0] + ptx[0]*tcos.x + pty[0]*tsin.x;
|
|
l = lz + lx*tcos.x + ly*tsin.x;
|
|
t.set((ccap-pt)*axis, l*axis);
|
|
bContact = inrange(((pt-ccap)*t.y+l*t.x).len2(), r2*sqr(t.y*(real)0.99),r2*sqr(t.y*(real)1.01)); // remove phantom roots
|
|
bContact &= inrange(t.x, (real)0,t.y);
|
|
bBest = isneg(tmax-tsin) & bContact;
|
|
UPDATE_IDBEST(tmax,0x40|icap+1>>1);
|
|
}
|
|
}
|
|
|
|
if (idbest<0)
|
|
return 0;
|
|
|
|
switch (idbest & 0xF0) {
|
|
case 0x00: // ray end - cyl cap
|
|
i = idbest & 1; icap = (idbest&2)-1;
|
|
pcontact->t = tmax.val();
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pcontact->pt = ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + pmode->center;
|
|
pcontact->n = axis*-icap;
|
|
pcontact->iFeature[0] = 0x80 | i;
|
|
pcontact->iFeature[1] = 0x41 + (icap+1>>1);
|
|
break;
|
|
case 0x10: // ray end - cyl side
|
|
i = idbest & 1;
|
|
pcontact->t = tmax.x;
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pcontact->pt = ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + pmode->center;
|
|
pcontact->n = pcyl->center-pcontact->pt;
|
|
(pcontact->n -= pcyl->axis*(pcyl->axis*pcontact->n)).normalize();
|
|
pcontact->iFeature[0] = 0x80 | i;
|
|
pcontact->iFeature[1] = 0x40;
|
|
break;
|
|
case 0x20: // ray - cyl side
|
|
pcontact->t = tmax.x;
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pt = ptz[0] + ptx[0]*pcontact->taux + pty[0]*pcontact->t;
|
|
l = lz + lx*pcontact->taux + ly*pcontact->t;
|
|
t.set((pt^axis)*(l^axis),(l^axis).len2());
|
|
pcontact->pt = pt+l*t.val()+pmode->center;
|
|
pcontact->n = (l^axis).normalized();
|
|
pcontact->n *= sgnnz(pcontact->n*(pcyl->center-pcontact->pt));
|
|
pcontact->iFeature[0] = 0x20;
|
|
pcontact->iFeature[1] = 0x40;
|
|
break;
|
|
case 0x40: // ray - cyl cap
|
|
icap = ((idbest&1)<<1)-1;
|
|
pcontact->t = tmax.x;
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pt = ptz[0] + ptx[0]*pcontact->taux + pty[0]*pcontact->t;
|
|
l = lz + lx*pcontact->taux + ly*pcontact->t;
|
|
ccap = center+axis*(hh*icap);
|
|
t.set((ccap-pt)*axis,l*axis);
|
|
pcontact->pt = pt+l*t.val();
|
|
pcontact->n = pcontact->pt-ccap; pcontact->pt += pmode->center;
|
|
pcontact->n = ((axis^pcontact->n)^l).normalized();
|
|
pcontact->n *= sgnnz(pcontact->n*(pcyl->center-pcontact->pt));
|
|
pcontact->iFeature[0] = 0x20;
|
|
pcontact->iFeature[1] = 0x20|icap+1>>1;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
int cyl_ray_rot_unprojection(unprojection_mode *pmode, const cylinder *pcyl,int iFeature1, const ray *pray,int iFeature2,
|
|
contact *pcontact, geom_contact_area *parea)
|
|
{
|
|
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
|
|
int res = ray_cyl_rot_unprojection(pmode,pray,iFeature2,pcyl,iFeature1,pcontact,parea);
|
|
if (res) {
|
|
pcontact->pt = pcontact->pt.rotated(pmode->center,pmode->dir, pcontact->taux,-pcontact->t);
|
|
pcontact->n = pcontact->n.rotated(pmode->dir, pcontact->taux,-pcontact->t).Flip();
|
|
int iFeature = pcontact->iFeature[0]; pcontact->iFeature[0]=pcontact->iFeature[1]; pcontact->iFeature[1]=iFeature;
|
|
}
|
|
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
|
|
return res;
|
|
}
|
|
|
|
|
|
|
|
int ray_box_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1, const box *pbox,int iFeature2,
|
|
contact *pcontact, geom_contact_area *parea)
|
|
{
|
|
int i,j,ifeat,idir,ix,iy,bContact,bBest,idbest=-1;
|
|
vectorr pt,ptz[2],ptx[2],pty[2],ptz0,ptx0,pty0,pt1,pt0_rot,pt1_rot,edge0_rot,ptz1,ptx1,pty1,origin,dir,center,rotax,n,size;
|
|
quotient tsin,tcos,tmax(0,1),t0,t1;
|
|
real kcos,ksin,k,a,b,c,d;
|
|
|
|
origin = pbox->Basis*(pray->origin-pbox->center);
|
|
dir = pbox->Basis*pray->dir;
|
|
center = pbox->Basis*(pmode->center-pbox->center);
|
|
rotax = pbox->Basis*pmode->dir;
|
|
size = pbox->size;
|
|
pt = origin-center; ptz[0] = rotax*(pt*rotax); ptx[0] = pt-ptz[0]; pty[0] = rotax^ptx[0];
|
|
pt += dir; ptz[1] = rotax*(pt*rotax); ptx[1] = pt-ptz[1]; pty[1] = rotax^ptx[1];
|
|
|
|
// ray end - box face
|
|
for(i=1;i>=0;i--) {
|
|
for(ifeat=0;ifeat<6;ifeat++) {
|
|
idir = ifeat>>1; ix = inc_mod3[idir]; iy = dec_mod3[idir];
|
|
kcos = ptx[i][idir]; ksin = pty[i][idir];
|
|
k = ptz[i][idir]-size[idir]*((ifeat&1)*2-1)+center[idir];
|
|
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
|
|
if (d>=0) {
|
|
d = sqrt_tpl(d); tsin.set(-b-d,a);
|
|
for(j=0;j<2;j++,tsin.x+=d*2)
|
|
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
|
|
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) = remove phantom roots
|
|
{
|
|
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
|
|
pt = ptx[i]*tcos.x + pty[i]*tsin.x + (ptz[i]+pmode->center)*tsin.y;
|
|
bContact = // point is inside the face
|
|
isneg(fabs_tpl(pt[ix])-size[ix]*tsin.y) & isneg(fabs_tpl(pt[iy])-size[iy]*tsin.y);
|
|
bBest = bContact & isneg(tmax-tsin);
|
|
UPDATE_IDBEST(tmax,i|ifeat<<1);
|
|
}
|
|
}
|
|
}
|
|
if ((pray->origin-pmode->center).len2()<sqr(pmode->minPtDist))
|
|
break;
|
|
}
|
|
|
|
// ray - box edge
|
|
pt = origin-center^dir; ptz0 = rotax*(pt*rotax); ptx0 = pt-ptz0; pty0 = rotax^ptx0;
|
|
for(ifeat=0;ifeat<12;ifeat++) {
|
|
idir = ifeat>>2; ix = inc_mod3[idir]; iy = dec_mod3[idir];
|
|
pt1[idir]=0; pt1[ix]=size[ix]*((ifeat&1)*2-1); pt1[iy]=size[iy]*((ifeat&2)-1);
|
|
pt1 = pt1-center;
|
|
pt = cross_with_ort(pt1,idir); ptz1 = rotax*(pt*rotax); ptx1 = pt-ptz1; pty1 = rotax^ptx1;
|
|
kcos = ptx0[idir]+dir*ptx1; ksin = pty0[idir]-dir*pty1; k = ptz0[idir]+dir*ptz1;
|
|
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
|
|
if (d>=0) {
|
|
d = sqrt_tpl(d); tsin.set(-b-d,a);
|
|
for(j=0;j<2;j++,tsin.x+=d*2)
|
|
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
|
|
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1)
|
|
{
|
|
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
|
|
pt0_rot = ptx[0]*tcos.x + pty[0]*tsin.x + ptz[0]*tsin.y;
|
|
pt1_rot = ptx[1]*tcos.x + pty[1]*tsin.x + ptz[1]*tsin.y;
|
|
edge0_rot = pt1_rot-pt0_rot;
|
|
n = cross_with_ort(edge0_rot,idir); pt = pt1*tsin.y-pt0_rot;
|
|
t0.set(cross_with_ort(pt,idir)*n, n.len2());
|
|
t1.set((pt^edge0_rot)*n, t0.y*tsin.y);
|
|
bContact = inrange(t0.x, (real)0,t0.y) & isneg(fabs_tpl(t1.x)-fabs_tpl(t1.y)*size[idir]);
|
|
bBest = bContact & isneg(tmax-tsin);
|
|
UPDATE_IDBEST(tmax,ifeat | 0x80);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (idbest<0)
|
|
return 0;
|
|
|
|
if ((idbest & 0x80)==0) {
|
|
i = idbest & 1; ifeat = idbest>>1; idir = ifeat>>1;
|
|
pcontact->t = tmax.val();
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pcontact->pt = (ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + center)*pbox->Basis + pbox->center;
|
|
pcontact->n.zero()[idir]=1-(ifeat&1)*2; pcontact->n = pcontact->n*pbox->Basis;
|
|
pcontact->iFeature[0] = 0x80 | i;
|
|
pcontact->iFeature[1] = 0x40 | ifeat;
|
|
} else {
|
|
ifeat = idbest&0x7F; idir = ifeat>>2; ix = inc_mod3[idir]; iy = dec_mod3[idir];
|
|
pcontact->t = tmax.val();
|
|
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
|
|
pt0_rot = ptx[0]*pcontact->taux + pty[0]*pcontact->t + ptz[0] + center;
|
|
pt1_rot = ptx[1]*pcontact->taux + pty[1]*pcontact->t + ptz[1] + center;
|
|
edge0_rot = pt1_rot-pt0_rot;
|
|
pt[idir]=0; pt[ix]=size[ix]*((ifeat&1)*2-1); pt[iy]=size[iy]*((ifeat&2)-1);
|
|
n = cross_with_ort(edge0_rot,idir);
|
|
t0.set(cross_with_ort(pt-pt0_rot,idir)*n, n.len2());
|
|
pt = pt0_rot + edge0_rot*t0.val();
|
|
pcontact->n = (n*-sgnnz(n*pt))*pbox->Basis;
|
|
pcontact->pt = pt*pbox->Basis+pbox->center;
|
|
pcontact->iFeature[0] = 0xA0;
|
|
pcontact->iFeature[1] = 0x20 | ifeat;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
int box_ray_rot_unprojection(unprojection_mode *pmode, const box *pbox,int iFeature1, const ray *pray,int iFeature2,
|
|
contact *pcontact, geom_contact_area *parea)
|
|
{
|
|
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
|
|
int res = ray_box_rot_unprojection(pmode,pray,iFeature2,pbox,iFeature1,pcontact,parea);
|
|
if (res) {
|
|
pcontact->pt = pcontact->pt.rotated(pmode->center,pmode->dir, pcontact->taux,-pcontact->t);
|
|
pcontact->n = pcontact->n.rotated(pmode->dir, pcontact->taux,-pcontact->t).Flip();
|
|
int iFeature = pcontact->iFeature[0]; pcontact->iFeature[0]=pcontact->iFeature[1]; pcontact->iFeature[1]=iFeature;
|
|
}
|
|
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
|
|
return res;
|
|
} |