static int geographic_edgelen (int i, int j, CCdatagroup *dat)
{
double deg, min;
double lati, latj, longi, longj;
double q1, q2, q3;
int dd;
double x1 = dat->x[i], x2 = dat->x[j], yy1 = dat->y[i], yy2 = dat->y[j];
deg = dtrunc (x1);
min = x1 - deg;
lati = GH_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc (x2);
min = x2 - deg;
latj = GH_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc (yy1);
min = yy1 - deg;
longi = GH_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc (yy2);
min = yy2 - deg;
longj = GH_PI * (deg + 5.0 * min / 3.0) / 180.0;
q1 = cos (longi - longj);
q2 = cos (lati - latj);
q3 = cos (lati + latj);
dd = (int) (6378.388 * acos (0.5 * ((1.0 + q1) * q2 - (1.0 - q1) * q3))
+ 1.0);
return dd;
}
long GeoDistance::compute(long i, long j, DataStructure::Point *nodeptr) {
double deg, min;
double lati, latj, longi, longj;
double q1, q2, q3;
long int dd;
double x1 = nodeptr[i].x, x2 = nodeptr[j].x, y1 = nodeptr[i].y, y2 =
nodeptr[j].y;
deg = dtrunc(x1);
min = x1 - deg;
lati = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc(x2);
min = x2 - deg;
latj = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc(y1);
min = y1 - deg;
longi = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc(y2);
min = y2 - deg;
longj = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
q1 = cos(longi - longj);
q2 = cos(lati - latj);
q3 = cos(lati + latj);
dd = (int) (6378.388 * acos(0.5 * ((1.0 + q1) * q2 - (1.0 - q1) * q3))
+ 1.0);
return dd;
}
globle void ModFunction(
void *theEnv,
DATA_OBJECT_PTR result)
{
DATA_OBJECT item1, item2;
double fnum1, fnum2;
long long lnum1, lnum2;
if (EnvArgCountCheck(theEnv,"mod",EXACTLY,2) == -1)
{
result->type = INTEGER;
result->value = (void *) EnvAddLong(theEnv,0L);
return;
}
if (EnvArgTypeCheck(theEnv,"mod",1,INTEGER_OR_FLOAT,&item1) == FALSE)
{
result->type = INTEGER;
result->value = (void *) EnvAddLong(theEnv,0L);
return;
}
if (EnvArgTypeCheck(theEnv,"mod",2,INTEGER_OR_FLOAT,&item2) == FALSE)
{
result->type = INTEGER;
result->value = (void *) EnvAddLong(theEnv,0L);
return;
}
if (((item2.type == INTEGER) ? (ValueToLong(item2.value) == 0L) : FALSE) ||
((item2.type == FLOAT) ? ValueToDouble(item2.value) == 0.0 : FALSE))
{
DivideByZeroErrorMessage(theEnv,"mod");
SetEvaluationError(theEnv,TRUE);
result->type = INTEGER;
result->value = (void *) EnvAddLong(theEnv,0L);
return;
}
if ((item1.type == FLOAT) || (item2.type == FLOAT))
{
fnum1 = CoerceToDouble(item1.type,item1.value);
fnum2 = CoerceToDouble(item2.type,item2.value);
result->type = FLOAT;
result->value = (void *) EnvAddDouble(theEnv,fnum1 - (dtrunc(fnum1 / fnum2) * fnum2));
}
else
{
lnum1 = DOToLong(item1);
lnum2 = DOToLong(item2);
result->type = INTEGER;
result->value = (void *) EnvAddLong(theEnv,lnum1 - (lnum1 / lnum2) * lnum2);
}
}
long int geo_distance (long int i, long int j)
/*
FUNCTION: compute geometric distance between two nodes rounded to next
integer for TSPLIB instances
INPUT: two node indices
OUTPUT: distance between the two nodes
COMMENTS: adapted from concorde code
for the definition of how to compute this distance see TSPLIB
*/
{
double deg, min;
double lati, latj, longi, longj;
double q1, q2, q3;
long int dd;
double x1 = instance.nodeptr[i].x, x2 = instance.nodeptr[j].x,
y1 = instance.nodeptr[i].y, y2 = instance.nodeptr[j].y;
deg = dtrunc (x1);
min = x1 - deg;
lati = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc (x2);
min = x2 - deg;
latj = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc (y1);
min = y1 - deg;
longi = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
deg = dtrunc (y2);
min = y2 - deg;
longj = M_PI * (deg + 5.0 * min / 3.0) / 180.0;
q1 = cos (longi - longj);
q2 = cos (lati - latj);
q3 = cos (lati + latj);
dd = (int) (6378.388 * acos (0.5 * ((1.0 + q1) * q2 - (1.0 - q1) * q3)) + 1.0);
return dd;
}
static int att_edgelen (int i, int j, CCdatagroup *dat)
{
double xd = dat->x[i] - dat->x[j];
double yd = dat->y[i] - dat->y[j];
double rij = sqrt ((xd * xd + yd * yd) / 10.0);
double tij = dtrunc (rij);
int dij;
if (tij < rij)
dij = (int) tij + 1;
else
dij = (int) tij;
return dij;
}
/* 0 y 2: brazo rojo; 1, 3: brazo negro
* El hack es porque el motor 1 empuja menos, o el 3 empuja más.
*/
void control_mixer(frac thrust, vec3 torque, struct motorData* output)
{
frac hack;
output->speed[0] = gammainv(thrust, 0, torque.y, -torque.z);
hack = gammainv(thrust, -torque.x, 0, torque.z);
output->speed[1] = dtrunc(fexpand(hack) + fmul2(hack, 6500));
output->speed[2] = gammainv(thrust, 0, -torque.y, -torque.z);
output->speed[3] = gammainv(thrust, torque.x, 0, torque.z);
return;
}
long int att_distance (long int i, long int j)
/*
FUNCTION: compute ATT distance between two nodes rounded to next
integer for TSPLIB instances
INPUT: two node indices
OUTPUT: distance between the two nodes
COMMENTS: for the definition of how to compute this distance see TSPLIB
*/
{
double xd = instance.nodeptr[i].x - instance.nodeptr[j].x;
double yd = instance.nodeptr[i].y - instance.nodeptr[j].y;
double rij = sqrt ((xd * xd + yd * yd) / 10.0);
double tij = dtrunc (rij);
long int dij;
if (tij < rij)
dij = (int) tij + 1;
else
dij = (int) tij;
return dij;
}
/*
* truncate d (in p) to length `newsize'.
* if larger, just increase size.
* if smaller, deallocate blocks after last one
* still in file at new size. last byte to keep
* is newsize-1, due to zero origin.
* we free in forward order because it's simpler to get right.
* if the final block at the new size is partially-filled,
* zero the remainder.
*/
int
dtrunclen(Iobuf *p, Dentry *d, Off newsize, int uid)
{
int i, pastlast;
Truncstate trunc;
if (newsize <= 0) {
dtrunc(p, d, uid);
return 0;
}
memset(&trunc, 0, sizeof trunc);
trunc.d = d;
trunc.p = p;
trunc.uid = uid;
trunc.newsize = newsize;
trunc.lastblk = newsize/BUFSIZE;
if (newsize % BUFSIZE == 0)
trunc.lastblk--;
else
trunczero(&trunc);
for (i = 0; i < NDBLOCK; i++)
if (trunc.pastlast) {
trunc.relblk = i;
buffree(p->dev, d->dblock[i], 0, &trunc);
d->dblock[i] = 0;
} else if (i == trunc.lastblk)
trunc.pastlast = 1;
trunc.relblk = NDBLOCK;
for (i = 0; i < NIBLOCK; i++) {
pastlast = trunc.pastlast;
buffree(p->dev, d->iblocks[i], i+1, &trunc);
if (pastlast)
d->iblocks[i] = 0;
}
d->size = newsize;
p->flags |= Bmod|Bimm;
accessdir(p, d, FWRITE, uid);
return trunc.err;
}
globle double PowFunction(
void *theEnv)
{
DATA_OBJECT value1, value2;
if (EnvArgCountCheck(theEnv,"**",EXACTLY,2) == -1) return(0.0);
if (EnvArgTypeCheck(theEnv,"**",1,FLOAT,&value1) == FALSE) return(0.0);
if (EnvArgTypeCheck(theEnv,"**",2,FLOAT,&value2) == FALSE) return(0.0);
if (((DOToDouble(value1) == 0.0) &&
(DOToDouble(value2) <= 0.0)) ||
((DOToDouble(value1) < 0.0) &&
(dtrunc((double) DOToDouble(value2)) != DOToDouble(value2))))
{
DomainErrorMessage(theEnv,"**");
SetHaltExecution(theEnv,TRUE);
SetEvaluationError(theEnv,TRUE);
return(0.0);
}
return (pow(DOToDouble(value1),DOToDouble(value2)));
}
int
doremove(File *f, int iscon)
{
Iobuf *p, *p1;
Dentry *d, *d1;
int32_t addr;
int slot, err;
p = 0;
p1 = 0;
if(isro(f->fs->dev) || (f->cp != cons.chan && writegroup && !ingroup(f->uid, writegroup))) {
err = Eronly;
goto out;
}
/*
* check on parent directory of file to be deleted
*/
if(f->wpath == 0 || f->wpath->addr == f->addr) {
err = Ephase;
goto out;
}
p1 = getbuf(f->fs->dev, f->wpath->addr, Bread);
d1 = getdir(p1, f->wpath->slot);
if(!d1 || checktag(p1, Tdir, QPNONE) || !(d1->mode & DALLOC)) {
err = Ephase;
goto out;
}
if(!iscon && iaccess(f, d1, DWRITE)) {
err = Eaccess;
goto out;
}
accessdir(p1, d1, FWRITE);
putbuf(p1);
p1 = 0;
/*
* check on file to be deleted
*/
p = getbuf(f->fs->dev, f->addr, Bread);
d = getdir(p, f->slot);
if(!d || checktag(p, Tdir, QPNONE) || !(d->mode & DALLOC)) {
err = Ealloc;
goto out;
}
if(err = mkqidcmp(&f->qid, d))
goto out;
/*
* if deleting a directory, make sure it is empty
*/
if((d->mode & DDIR))
for(addr=0;; addr++) {
p1 = dnodebuf(p, d, addr, 0);
if(!p1)
break;
if(checktag(p1, Tdir, d->qid.path)) {
err = Ephase;
goto out;
}
for(slot=0; slot<DIRPERBUF; slot++) {
d1 = getdir(p1, slot);
if(!(d1->mode & DALLOC))
continue;
err = Eempty;
goto out;
}
putbuf(p1);
}
/*
* do it
*/
dtrunc(p, d);
memset(d, 0, sizeof(Dentry));
settag(p, Tdir, QPNONE);
out:
if(p1)
putbuf(p1);
if(p)
putbuf(p);
return err;
}
void
f_open(Chan *cp, Oldfcall *in, Oldfcall *ou)
{
Iobuf *p;
Dentry *d;
File *f;
Tlock *t;
Qid qid;
int ro, fmod;
if(CHAT(cp)) {
print("c_open %d\n", cp->chan);
print(" fid = %d\n", in->fid);
print(" mode = %o\n", in->mode);
}
p = 0;
f = filep(cp, in->fid, 0);
if(!f) {
ou->err = Efid;
goto out;
}
/*
* if remove on close, check access here
*/
ro = isro(f->fs->dev) || (cp != cons.chan && writegroup && !ingroup(f->uid, writegroup));
if(in->mode & MRCLOSE) {
if(ro) {
ou->err = Eronly;
goto out;
}
/*
* check on parent directory of file to be deleted
*/
if(f->wpath == 0 || f->wpath->addr == f->addr) {
ou->err = Ephase;
goto out;
}
p = getbuf(f->fs->dev, f->wpath->addr, Bread);
d = getdir(p, f->wpath->slot);
if(!d || checktag(p, Tdir, QPNONE) || !(d->mode & DALLOC)) {
ou->err = Ephase;
goto out;
}
if(iaccess(f, d, DWRITE)) {
ou->err = Eaccess;
goto out;
}
putbuf(p);
}
p = getbuf(f->fs->dev, f->addr, Bread);
d = getdir(p, f->slot);
if(!d || checktag(p, Tdir, QPNONE) || !(d->mode & DALLOC)) {
ou->err = Ealloc;
goto out;
}
if(ou->err = mkqidcmp(&f->qid, d))
goto out;
mkqid(&qid, d, 1);
switch(in->mode & 7) {
case MREAD:
if(iaccess(f, d, DREAD) && !writeallow)
goto badaccess;
fmod = FREAD;
break;
case MWRITE:
if((d->mode & DDIR) ||
(iaccess(f, d, DWRITE) && !writeallow))
goto badaccess;
if(ro) {
ou->err = Eronly;
goto out;
}
fmod = FWRITE;
break;
case MBOTH:
if((d->mode & DDIR) ||
(iaccess(f, d, DREAD) && !writeallow) ||
(iaccess(f, d, DWRITE) && !writeallow))
goto badaccess;
if(ro) {
ou->err = Eronly;
goto out;
}
fmod = FREAD+FWRITE;
break;
case MEXEC:
if((d->mode & DDIR) ||
iaccess(f, d, DEXEC))
goto badaccess;
fmod = FREAD;
break;
default:
ou->err = Emode;
goto out;
}
if(in->mode & MTRUNC) {
if((d->mode & DDIR) ||
(iaccess(f, d, DWRITE) && !writeallow))
goto badaccess;
if(ro) {
ou->err = Eronly;
goto out;
}
}
t = 0;
if(d->mode & DLOCK) {
t = tlocked(p, d);
if(t == 0) {
ou->err = Elocked;
goto out;
}
t->file = f;
}
if(in->mode & MRCLOSE)
fmod |= FREMOV;
f->open = fmod;
if(in->mode & MTRUNC)
if(!(d->mode & DAPND))
dtrunc(p, d);
f->tlock = t;
f->lastra = 0;
mkqid9p1(&ou->qid, &qid);
goto out;
badaccess:
ou->err = Eaccess;
f->open = 0;
out:
if(p)
putbuf(p);
if(f)
qunlock(f);
ou->fid = in->fid;
}