/*
* ppb_MS_loop()
*
* Execute a microseq loop
*
*/
int
ppb_MS_loop(device_t bus, device_t dev, struct ppb_microseq *prolog,
struct ppb_microseq *body, struct ppb_microseq *epilog,
int iter, int *ret)
{
struct ppb_microseq loop_microseq[] = {
MS_CALL(0), /* execute prolog */
MS_SET(MS_UNKNOWN), /* set size of transfer */
/* loop: */
MS_CALL(0), /* execute body */
MS_DBRA(-1 /* loop: */),
MS_CALL(0), /* execute epilog */
MS_RET(0)
};
/* initialize the structure */
loop_microseq[0].arg[0].p = (void *)prolog;
loop_microseq[1].arg[0].i = iter;
loop_microseq[2].arg[0].p = (void *)body;
loop_microseq[4].arg[0].p = (void *)epilog;
/* execute the loop */
return (ppb_MS_microseq(bus, dev, loop_microseq, ret));
}
static char
vpoio_select(struct vpoio_data *vpo, int initiator, int target)
{
device_t ppbus = device_get_parent(vpo->vpo_dev);
int ret;
struct ppb_microseq select_microseq[] = {
/* parameter list
*/
#define SELECT_TARGET MS_PARAM(0, 1, MS_TYP_INT)
#define SELECT_INITIATOR MS_PARAM(3, 1, MS_TYP_INT)
/* send the select command to the drive */
MS_DASS(MS_UNKNOWN),
MS_CASS(H_nAUTO | H_nSELIN | H_INIT | H_STROBE),
MS_CASS( H_AUTO | H_nSELIN | H_INIT | H_STROBE),
MS_DASS(MS_UNKNOWN),
MS_CASS( H_AUTO | H_nSELIN | H_nINIT | H_STROBE),
/* now, wait until the drive is ready */
MS_SET(VP0_SELTMO),
/* loop: */ MS_BRSET(H_ACK, 2 /* ready */),
MS_DBRA(-2 /* loop */),
/* error: */ MS_RET(1),
/* ready: */ MS_RET(0)
};
/* initialize the select microsequence */
ppb_MS_init_msq(select_microseq, 2,
SELECT_TARGET, 1 << target,
SELECT_INITIATOR, 1 << initiator);
ppb_MS_microseq(ppbus, vpo->vpo_dev, select_microseq, &ret);
if (ret)
return (VP0_ESELECT_TIMEOUT);
return (0);
}
/*
* ppb_MS_microseq()
*
* Interprete a microsequence. Some microinstructions are executed at adapter
* level to avoid function call overhead between ppbus and the adapter
*/
int
ppb_MS_microseq(device_t bus, device_t dev, struct ppb_microseq *msq, int *ret)
{
struct ppb_data *ppb = (struct ppb_data *)device_get_softc(bus);
struct ppb_device *ppbdev = (struct ppb_device *)device_get_ivars(dev);
struct ppb_microseq *mi; /* current microinstruction */
int error;
struct ppb_xfer *xfer;
/* microsequence executed to initialize the transfer */
struct ppb_microseq initxfer[] = {
MS_PTR(MS_UNKNOWN), /* set ptr to buffer */
MS_SET(MS_UNKNOWN), /* set transfer size */
MS_RET(0)
};
if (ppb->ppb_owner != dev)
return (EACCES);
#define INCR_PC (mi ++)
mi = msq;
for (;;) {
switch (mi->opcode) {
case MS_OP_PUT:
case MS_OP_GET:
/* attempt to choose the best mode for the device */
xfer = mode2xfer(bus, ppbdev, mi->opcode);
/* figure out if we should use ieee1284 code */
if (!xfer->loop) {
if (mi->opcode == MS_OP_PUT) {
if ((error = PPBUS_WRITE(
device_get_parent(bus),
(char *)mi->arg[0].p,
mi->arg[1].i, 0)))
goto error;
INCR_PC;
goto next;
} else
panic("%s: IEEE1284 read not supported", __func__);
}
/* XXX should use ppb_MS_init_msq() */
initxfer[0].arg[0].p = mi->arg[0].p;
initxfer[1].arg[0].i = mi->arg[1].i;
/* initialize transfer */
ppb_MS_microseq(bus, dev, initxfer, &error);
if (error)
goto error;
/* the xfer microsequence should not contain any
* MS_OP_PUT or MS_OP_GET!
*/
ppb_MS_microseq(bus, dev, xfer->loop, &error);
if (error)
goto error;
INCR_PC;
break;
case MS_OP_RET:
if (ret)
*ret = mi->arg[0].i; /* return code */
return (0);
break;
default:
/* executing microinstructions at ppc level is
* faster. This is the default if the microinstr
* is unknown here
*/
if ((error = PPBUS_EXEC_MICROSEQ(
device_get_parent(bus), &mi)))
goto error;
break;
}
next:
continue;
}
error:
return (error);
}
/* check if edge need to be split and return a binary coding the numbers of the edges of tria iel
that should be split according to a hausdorff distance criterion */
int chkedg(pMesh mesh,int iel) {
pTria pt;
pPoint p[3];
double n[3][3],t[3][3],nt[3],c1[3],c2[3],*n1,*n2,t1[3],t2[3];
double ps,ps2,cosn,ux,uy,uz,ll,li,dd;
char i,i1,i2;
pt = &mesh->tria[iel];
p[0] = &mesh->point[pt->v[0]];
p[1] = &mesh->point[pt->v[1]];
p[2] = &mesh->point[pt->v[2]];
/* normal recovery */
for (i=0; i<3; i++) {
if ( MS_SIN(p[i]->tag) ) {
nortri(mesh,pt,n[i]);
}
else if ( MS_EDG(p[i]->tag) ) {
nortri(mesh,pt,nt);
n1 = &mesh->geom[p[i]->ig].n1[0];
n2 = &mesh->geom[p[i]->ig].n2[0];
ps = n1[0]*nt[0] + n1[1]*nt[1] + n1[2]*nt[2];
ps2 = n2[0]*nt[0] + n2[1]*nt[1] + n2[2]*nt[2];
if ( fabs(ps) > fabs(ps2) )
memcpy(&n[i],n1,3*sizeof(double));
else
memcpy(&n[i],n2,3*sizeof(double));
memcpy(&t[i],p[i]->n,3*sizeof(double));
}
else
memcpy(&n[i],p[i]->n,3*sizeof(double));
}
/* analyze edges */
for (i=0; i<3; i++) {
i1 = inxt[i];
i2 = iprv[i];
/* check length */
ux = p[i2]->c[0] - p[i1]->c[0];
uy = p[i2]->c[1] - p[i1]->c[1];
uz = p[i2]->c[2] - p[i1]->c[2];
ll = ux*ux + uy*uy + uz*uz;
if ( ll > info.hmax*info.hmax ) {
MS_SET(pt->flag,i);
continue;
}
else if ( !MS_EDG(pt->tag[i]) && p[i1]->tag > MS_NOTAG && p[i2]->tag > MS_NOTAG ) {
MS_SET(pt->flag,i);
continue;
}
/* Hausdorff w/r tangent direction */
if ( MS_EDG(pt->tag[i]) ) {
if ( MS_SIN(p[i1]->tag) ) {
li = 1.0 / sqrt(ll);
t1[0] = li*ux;
t1[1] = li*uy;
t1[2] = li*uz;
}
else{
memcpy(t1,t[i1],3*sizeof(double));
}
if ( MS_SIN(p[i2]->tag) ) {
li = 1.0 / sqrt(ll);
t2[0] = li*ux;
t2[1] = li*uy;
t2[2] = li*uz;
}
else{
memcpy(t2,t[i2],3*sizeof(double));
}
ps = t1[0]*ux + t1[1]*uy + t1[2]*uz;
ps *= ps;
cosn = ps/ll ;
cosn *= (1.0-cosn);
cosn *= (0.25*ll);
if ( cosn > info.hausd*info.hausd ) {
MS_SET(pt->flag,i);
continue;
}
ps = -(t2[0]*ux + t2[1]*uy + t2[2]*uz);
ps *= ps;
cosn = ps/ll ;
cosn *= (1.0-cosn);
cosn *= (0.25*ll);
if ( cosn > info.hausd*info.hausd ) {
MS_SET(pt->flag,i);
continue;
}
}
else {
n1 = n[i1];
n2 = n[i2];
ps = ux*n1[0] + uy*n1[1] + uz*n1[2];
c1[0] = (2.0*p[i1]->c[0] + p[i2]->c[0] - ps*n1[0]) / 3.0 - p[i1]->c[0];
c1[1] = (2.0*p[i1]->c[1] + p[i2]->c[1] - ps*n1[1]) / 3.0 - p[i1]->c[1];
c1[2] = (2.0*p[i1]->c[2] + p[i2]->c[2] - ps*n1[2]) / 3.0 - p[i1]->c[2];
ps = -(ux*n2[0] + uy*n2[1] + uz*n2[2]);
c2[0] = (2.0*p[i2]->c[0] + p[i1]->c[0] - ps*n2[0]) / 3.0 - p[i2]->c[0];
c2[1] = (2.0*p[i2]->c[1] + p[i1]->c[1] - ps*n2[1]) / 3.0 - p[i2]->c[1];
c2[2] = (2.0*p[i2]->c[2] + p[i1]->c[2] - ps*n2[2]) / 3.0 - p[i2]->c[2];
/* squared cosines */
ps = c1[0]*ux + c1[1]*uy + c1[2]*uz;
ps *= ps;
dd = c1[0]*c1[0] + c1[1]*c1[1] + c1[2]*c1[2];
cosn = ps / (dd*ll);
cosn *= (1.0-cosn);
cosn *= (0.25*ll);
if ( cosn > info.hausd*info.hausd ) {
MS_SET(pt->flag,i);
continue;
}
ps = -c2[0]*ux - c2[1]*uy - c2[2]*uz;
ps *= ps;
dd = c2[0]*c2[0]+c2[1]*c2[1]+c2[2]*c2[2];
cosn = ps / (dd*ll);
cosn *= (1.0-cosn);
cosn *= (0.25*ll);
if ( cosn > info.hausd*info.hausd ) {
MS_SET(pt->flag,i);
continue;
}
}
}
return(pt->flag);
}
/* analyze triangles and split if needed */
static int anaelt(pMesh mesh,pSol met,char typchk) {
pTria pt;
pPoint ppt,p1,p2;
Hash hash;
Bezier pb;
pGeom go;
double s,o[3],no[3],to[3],dd,len;
int vx[3],i,j,ip,ip1,ip2,ier,k,ns,nc,nt;
char i1,i2;
static double uv[3][2] = { {0.5,0.5}, {0.,0.5}, {0.5,0.} };
hashNew(&hash,mesh->np);
ns = 0;
s = 0.5;
for (k=1; k<=mesh->nt; k++) {
pt = &mesh->tria[k];
if ( !MS_EOK(pt) || pt->ref < 0 ) continue;
if ( MS_SIN(pt->tag[0]) || MS_SIN(pt->tag[1]) || MS_SIN(pt->tag[2]) ) continue;
/* check element cut */
pt->flag = 0;
if ( typchk == 1 ) {
if ( !chkedg(mesh,k) ) continue;
}
else if ( typchk == 2 ) {
for (i=0; i<3; i++) {
i1 = inxt[i];
i2 = iprv[i];
len = lenedg(mesh,met,pt->v[i1],pt->v[i2],0);
if ( len > LLONG ) MS_SET(pt->flag,i);
}
if ( !pt->flag ) continue;
}
ns++;
/* geometric support */
ier = bezierCP(mesh,k,&pb);
assert(ier);
/* scan edges to split */
for (i=0; i<3; i++) {
if ( !MS_GET(pt->flag,i) ) continue;
i1 = inxt[i];
i2 = iprv[i];
ip1 = pt->v[i1];
ip2 = pt->v[i2];
ip = hashGet(&hash,ip1,ip2);
if ( !MS_EDG(pt->tag[i]) && ip > 0 ) continue;
/* new point along edge */
ier = bezierInt(&pb,uv[i],o,no,to);
if ( !ip ) {
ip = newPt(mesh,o,MS_EDG(pt->tag[i]) ? to : no);
assert(ip);
hashEdge(&hash,ip1,ip2,ip);
p1 = &mesh->point[ip1];
p2 = &mesh->point[ip2];
ppt = &mesh->point[ip];
if ( MS_EDG(pt->tag[i]) ) {
++mesh->ng;
assert(mesh->ng < mesh->ngmax);
ppt->tag = pt->tag[i];
if ( p1->ref == pt->edg[i] || p2->ref == pt->edg[i] )
ppt->ref = pt->edg[i];
ppt->ig = mesh->ng;
go = &mesh->geom[mesh->ng];
memcpy(go->n1,no,3*sizeof(double));
dd = go->n1[0]*ppt->n[0] + go->n1[1]*ppt->n[1] + go->n1[2]*ppt->n[2];
ppt->n[0] -= dd*go->n1[0];
ppt->n[1] -= dd*go->n1[1];
ppt->n[2] -= dd*go->n1[2];
dd = ppt->n[0]*ppt->n[0] + ppt->n[1]*ppt->n[1] + ppt->n[2]*ppt->n[2];
if ( dd > EPSD2 ) {
dd = 1.0 / sqrt(dd);
ppt->n[0] *= dd;
ppt->n[1] *= dd;
ppt->n[2] *= dd;
}
}
if ( met->m ) {
if ( typchk == 1 )
intmet33(mesh,met,ip1,ip2,ip,s);
else
intmet(mesh,met,k,i,ip,s);
}
}
else if ( pt->tag[i] & MS_GEO ) {
ppt = &mesh->point[ip];
go = &mesh->geom[ppt->ig];
memcpy(go->n2,no,3*sizeof(double));
/* a computation of the tangent with respect to these two normals is possible */
ppt->n[0] = go->n1[1]*go->n2[2] - go->n1[2]*go->n2[1];
ppt->n[1] = go->n1[2]*go->n2[0] - go->n1[0]*go->n2[2];
ppt->n[2] = go->n1[0]*go->n2[1] - go->n1[1]*go->n2[0];
dd = ppt->n[0]*ppt->n[0] + ppt->n[1]*ppt->n[1] + ppt->n[2]*ppt->n[2];
if ( dd > EPSD2 ) {
dd = 1.0 / sqrt(dd);
ppt->n[0] *= dd;
ppt->n[1] *= dd;
ppt->n[2] *= dd;
}
}
}
}
if ( !ns ) {
free(hash.item);
return(ns);
}
/* step 2. checking if split by adjacent */
for (k=1; k<=mesh->nt; k++) {
pt = &mesh->tria[k];
if ( !MS_EOK(pt) || pt->ref < 0 ) continue;
else if ( pt->flag == 7 ) continue;
/* geometric support */
ier = bezierCP(mesh,k,&pb);
assert(ier);
nc = 0;
for (i=0; i<3; i++) {
i1 = inxt[i];
i2 = inxt[i1];
if ( !MS_GET(pt->flag,i) && !MS_SIN(pt->tag[i]) ) {
ip = hashGet(&hash,pt->v[i1],pt->v[i2]);
if ( ip > 0 ) {
MS_SET(pt->flag,i);
nc++;
if ( pt->tag[i] & MS_GEO ) {
/* new point along edge */
ier = bezierInt(&pb,uv[i],o,no,to);
assert(ier);
ppt = &mesh->point[ip];
go = &mesh->geom[ppt->ig];
memcpy(go->n2,no,3*sizeof(double));
/* a computation of the tangent with respect to these two normals is possible */
ppt->n[0] = go->n1[1]*go->n2[2] - go->n1[2]*go->n2[1];
ppt->n[1] = go->n1[2]*go->n2[0] - go->n1[0]*go->n2[2];
ppt->n[2] = go->n1[0]*go->n2[1] - go->n1[1]*go->n2[0];
dd = ppt->n[0]*ppt->n[0] + ppt->n[1]*ppt->n[1] + ppt->n[2]*ppt->n[2];
if ( dd > EPSD2 ) {
dd = 1.0 / sqrt(dd);
ppt->n[0] *= dd;
ppt->n[1] *= dd;
ppt->n[2] *= dd;
}
}
}
}
}
if ( nc > 0 ) ++ns;
}
if ( info.ddebug && ns ) {
fprintf(stdout," %d analyzed %d proposed\n",mesh->nt,ns);
fflush(stdout);
}
/* step 3. splitting */
ns = 0;
nt = mesh->nt;
for (k=1; k<=nt; k++) {
pt = &mesh->tria[k];
if ( !MS_EOK(pt) || pt->ref < 0 ) continue;
else if ( pt->flag == 0 ) continue;
j = -1;
vx[0] = vx[1] = vx[2] = 0;
for (i=0; i<3; i++) {
i1 = inxt[i];
i2 = inxt[i1];
if ( MS_GET(pt->flag,i) ) {
vx[i] = hashGet(&hash,pt->v[i1],pt->v[i2]);
assert(vx[i]);
j = i;
}
}
if ( pt->flag == 1 || pt->flag == 2 || pt->flag == 4 ) {
ier = split1(mesh,met,k,j,vx);
assert(ier);
ns++;
}
else if ( pt->flag == 7 ) {
ier = split3(mesh,met,k,vx);
assert(ier);
ns++;
}
else {
ier = split2(mesh,met,k,vx);
assert(ier);
ns++;
}
}
if ( (info.ddebug || abs(info.imprim) > 5) && ns > 0 )
fprintf(stdout," %7d splitted\n",ns);
free(hash.item);
return(ns);
}