void Robot::Prepare(float x1, float y1, float vel, float velK, float x2, float y2){
printf("\nPreparing! IDID %d", id);
float yDiff= y1-y2;
float xDiff= x1-x2;
float angle = 9999999;
float R = sqrt((yDiff*yDiff)+(xDiff*xDiff));
float dist = R;
int standoffDist = (int)(dist/10.0);
if(R > 50){
angle = acos(-xDiff/R);
if(yDiff < 0)
angle = 2*PI - angle;
}
float new_xDiff = -(standoffDist)*cos(angle);
float new_yDiff = (standoffDist)*sin(angle);
float new_x;
float new_y;
angle = 2*PI - angle;
printf("\nAngle to shoot is: %f",angle);
new_x = new_xDiff + x1;
new_y = new_yDiff + y1;
float diff = angle - orientation;
R = sqrt(pow((x - new_x),2)+pow((y-new_y),2));
if(sqrt(pow(diff,2)) < 0.05 && velT == 0 && velN == 0)
{
velT = 5;
velN = 0;
velA = 0;
Kick(velK);
printf("\nTIME TO SHOOT");
usleep(standoffDist*(5.0/4.0)*1000); //time taken to move in for kick
velT = -5;
usleep(standoffDist*(1000.0/dist)*1000); //time used to pull back / brake
velT = 0;
}
if(velT != 5){
Rotate(angle);
printf("%d is WEIRD, %d is WEIRD", id, id);
printf("\nnewX is %f, newY is %f + %d this is in Prepare",new_x,new_y,id);
if(R > 100)
PathTo(new_x,new_y,vel);
else
Move(new_x,new_y,vel);
}
printf("Exited Prepare! %d",id);
}
int main(int argc, char *argv[])
{ HITS_DB db;
FILE *nxfile, *ixfile;
char *pwd, *root;
int i;
if (argc != 2)
{ fprintf(stderr,"Usage: %s <path:db>\n",argv[0]);
exit (1);
}
pwd = PathTo(argv[1]);
root = Root(argv[1],".db");
ixfile = Fopen(Catenate(pwd,PATHSEP,root,".idx"),"r");
nxfile = Fopen(Catenate(pwd,PATHSEP,root,".ndx"),"w");
if (ixfile == NULL || nxfile == NULL)
exit (1);
free(pwd);
free(root);
if (fread(&db,sizeof(HITS_DB),1,ixfile) != 1)
SYSTEM_ERROR
fwrite(&db,sizeof(HITS_DB),1,nxfile);
fprintf(stderr,"Converting %d reads\n",db.ureads);
fflush(stderr);
for (i = 0; i < db.ureads; i++)
{ HITS_OLD orec;
HITS_READ nrec;
if (i%10000 == 0)
{ fprintf(stderr," Processing %d\n",i);
fflush(stderr);
}
if (fread(&orec,sizeof(HITS_OLD),1,ixfile) != 1)
SYSTEM_ERROR
nrec.origin = orec.origin;
nrec.fpulse = orec.beg;
nrec.rlen = orec.end-orec.beg;
nrec.boff = orec.boff;
nrec.coff = orec.coff;
nrec.flags = orec.flags;
fwrite(&nrec,sizeof(HITS_READ),1,nxfile);
}
fclose(ixfile);
fclose(nxfile);
exit (0);
}
bool CPathFind::PathAround(position_t point, float distance, uint8 pathFlags)
{
position_t* lastPoint = &point;
float randomRadian = RandomNumber() * M_PI * 2.0f;
lastPoint->x += cosf(randomRadian) * distance;
lastPoint->z += sinf(randomRadian) * distance;
return PathTo(point, pathFlags);
}
int main(int argc, char *argv[])
{ HITS_DB db;
FILE *nxfile, *ixfile;
char *pwd, *root;
int i;
if (argc != 2)
{ fprintf(stderr,"Usage: %s <path:db>\n",argv[0]);
exit (1);
}
pwd = PathTo(argv[1]);
root = Root(argv[1],".db");
ixfile = Fopen(Catenate(pwd,PATHSEP,root,".idx"),"r");
nxfile = Fopen(Catenate(pwd,PATHSEP,root,".ndx"),"w");
if (ixfile == NULL || nxfile == NULL)
exit (1);
free(pwd);
free(root);
if (fread(&db,sizeof(HITS_DB),1,ixfile) != 1)
SYSTEM_ERROR
fwrite(&db,sizeof(HITS_DB),1,nxfile);
for (i = 0; i < db.oreads; i++)
{ HITS_OLD orec;
HITS_NEW nrec;
if (fread(&orec,sizeof(HITS_OLD),1,ixfile) != 1)
SYSTEM_ERROR
nrec.origin = orec.origin;
nrec.beg = orec.beg;
nrec.end = orec.end;
nrec.boff = orec.boff;
nrec.coff = orec.coff;
nrec.flags = orec.flags;
fwrite(&nrec,sizeof(HITS_NEW),1,nxfile);
}
fclose(ixfile);
fclose(nxfile);
exit (0);
}
bool CPathFind::PathAround(position_t point, float distanceFromPoint, uint8 pathFlags)
{
Clear();
position_t* lastPoint = &point;
float randomRadian = dsprand::GetRandomNumber<float>(0, 2 * M_PI);
lastPoint->x += cosf(randomRadian) * distanceFromPoint;
lastPoint->z += sinf(randomRadian) * distanceFromPoint;
// save for sliding logic
m_originalPoint = point;
m_distanceFromPoint = 1;
// Don't clear path so
// original point / distance are kept
return PathTo(point, pathFlags, false);
}
int main(int argc, char *argv[])
{ char *prefix;
FILE *aout, *dout;
int VERBOSE;
// Process arguments
{ int i, j, k;
int flags[128];
ARG_INIT("Catrack")
j = 1;
for (i = 1; i < argc; i++)
if (argv[i][0] == '-')
{ ARG_FLAGS("v") }
else
argv[j++] = argv[i];
argc = j;
VERBOSE = flags['v'];
if (argc != 3)
{ fprintf(stderr,"Usage: %s %s\n",Prog_Name,Usage);
exit (1);
}
}
{ char *pwd, *root;
int plen;
plen = strlen(argv[1]);
if (strcmp(argv[1]+(plen-3),".dam") == 0)
root = Root(argv[1],".dam");
else
root = Root(argv[1],".db");
pwd = PathTo(argv[1]);
prefix = Strdup(Catenate(pwd,PATHSEP,root,"."),"Allocating track name");
free(pwd);
free(root);
aout = fopen(Catenate(prefix,argv[2],".","anno"),"r");
if (aout != NULL)
{ fprintf(stderr,"%s: Track file %s%s.anno already exists!\n",Prog_Name,prefix,argv[2]);
fclose(aout);
exit (1);
}
dout = fopen(Catenate(prefix,argv[2],".","data"),"r");
if (dout != NULL)
{ fprintf(stderr,"%s: Track file %s%s.data already exists!\n",Prog_Name,prefix,argv[2]);
fclose(dout);
exit (1);
}
aout = Fopen(Catenate(prefix,argv[2],".","anno"),"w");
if (aout == NULL)
exit (1);
dout = NULL;
}
{ int tracktot, tracksiz;
int64 trackoff;
int nfiles;
char data[1024];
void *anno;
anno = NULL;
trackoff = 0;
tracktot = tracksiz = 0;
fwrite(&tracktot,sizeof(int),1,aout);
fwrite(&tracksiz,sizeof(int),1,aout);
nfiles = 0;
while (1)
{ FILE *afile, *dfile;
int i, size, tracklen;
afile = fopen(Numbered_Suffix(prefix,nfiles+1,Catenate(".",argv[2],".","anno")),"r");
if (afile == NULL)
break;
dfile = fopen(Numbered_Suffix(prefix,nfiles+1,Catenate(".",argv[2],".","data")),"r");
if (VERBOSE)
{ fprintf(stderr,"Concatenating %s%d.%s ...\n",prefix,nfiles+1,argv[2]);
fflush(stderr);
}
if (fread(&tracklen,sizeof(int),1,afile) != 1)
SYSTEM_ERROR
if (fread(&size,sizeof(int),1,afile) != 1)
SYSTEM_ERROR
if (nfiles == 0)
{ tracksiz = size;
if (dfile != NULL)
{ dout = Fopen(Catenate(prefix,argv[2],".","data"),"w");
if (dout == NULL)
{ fclose(afile);
fclose(dfile);
goto error;
}
}
else
{ anno = Malloc(size,"Allocating annotation record");
if (anno == NULL)
{ fclose(afile);
goto error;
}
}
}
else
{ int escape = 1;
if (tracksiz != size)
{ fprintf(stderr,"%s: Track block %d does not have the same annotation size (%d)",
Prog_Name,nfiles+1,size);
fprintf(stderr," as previous blocks (%d)\n",tracksiz);
}
else if (dfile == NULL && dout != NULL)
fprintf(stderr,"%s: Track block %d does not have data but previous blocks do\n",
Prog_Name,nfiles+1);
else if (dfile != NULL && dout == NULL)
fprintf(stderr,"%s: Track block %d has data but previous blocks do not\n",
Prog_Name,nfiles+1);
else
escape = 0;
if (escape)
{ fclose(afile);
if (dfile != NULL) fclose(dfile);
if (anno != NULL) free(anno);
goto error;
}
}
if (dfile != NULL)
{ int64 dlen;
if (size == 4)
{ int anno4;
for (i = 0; i < tracklen; i++)
{ if (fread(&anno4,sizeof(int),1,afile) != 1)
SYSTEM_ERROR
anno4 += trackoff;
fwrite(&anno4,sizeof(int),1,aout);
}
if (fread(&anno4,sizeof(int),1,afile) != 1)
SYSTEM_ERROR
dlen = anno4;
}
else
{ int64 anno8;
for (i = 0; i < tracklen; i++)
{ if (fread(&anno8,sizeof(int64),1,afile) != 1)
SYSTEM_ERROR
anno8 += trackoff;
fwrite(&anno8,sizeof(int64),1,aout);
}
if (fread(&anno8,sizeof(int64),1,afile) != 1)
SYSTEM_ERROR
dlen = anno8;
}
trackoff += dlen;
for (i = 1024; i < dlen; i += 1024)
{ if (fread(data,1024,1,dfile) != 1)
SYSTEM_ERROR
fwrite(data,1024,1,dout);
}
i -= 1024;
if (i < dlen)
{ if (fread(data,dlen-i,1,dfile) != 1)
SYSTEM_ERROR
fwrite(data,dlen-i,1,dout);
}
}
else
{ for (i = 0; i < tracklen; i++)
{ if (fread(anno,size,1,afile) != 1)
SYSTEM_ERROR
fwrite(anno,size,1,aout);
}
}
tracktot += tracklen;
nfiles += 1;
if (dfile != NULL) fclose(dfile);
fclose(afile);
}
if (nfiles == 0)
{ fprintf(stderr,"%s: Couldn't find first track block %s1.%s.anno\n",
Prog_Name,prefix,argv[2]);
goto error;
}
else
{ if (dout != NULL)
{ if (tracksiz == 4)
{ int anno4 = trackoff;
fwrite(&anno4,sizeof(int),1,aout);
}
else
{ int64 anno8 = trackoff;
fwrite(&anno8,sizeof(int64),1,aout);
}
}
else
{ fwrite(anno,tracksiz,1,aout);
free(anno);
}
rewind(aout);
fwrite(&tracktot,sizeof(int),1,aout);
fwrite(&tracksiz,sizeof(int),1,aout);
}
}
fclose(aout);
if (dout != NULL)
fclose(dout);
free(prefix);
exit (0);
error:
fclose(aout);
unlink(Catenate(prefix,argv[2],".","anno"));
if (dout != NULL)
{ fclose(dout);
unlink(Catenate(prefix,argv[2],".","data"));
}
free(prefix);
exit (1);
}
int main(int argc, char *argv[])
{ DAZZ_DB db;
int status;
Prog_Name = Strdup("DBwipe","Allocating Program Name");
if (argc != 2)
{ fprintf(stderr,"Usage: %s %s\n",Prog_Name,Usage);
exit (1);
}
// Open db
status = Open_DB(argv[1],&db);
if (status < 0)
exit (1);
if (db.part > 0)
{ fprintf(stderr,"%s: Cannot be called on a block: %s\n",Prog_Name,argv[1]);
exit (1);
}
if (status)
{ fprintf(stderr,"%s: Cannot be called on a .dam: %s\n",Prog_Name,argv[1]);
exit (1);
}
{ char *pwd, *root;
FILE *index;
char *index_name;
int i;
pwd = PathTo(argv[1]);
root = Root(argv[1],".db");
if (unlink(Catenate(pwd,PATHSEP,root,".arw")) < 0)
{ if (errno != ENOENT)
{ fprintf(stderr,"%s: [WARNING] Could not delete %s.arw\n",Prog_Name,root);
exit (1);
}
}
if (unlink(Catenate(pwd,PATHSEP,root,".qvs")) < 0)
{ if (errno != ENOENT)
{ fprintf(stderr,"%s: [WARNING] Could not delete %s.qvs\n",Prog_Name,root);
exit (1);
}
}
for (i = 0; i < db.nreads; i++)
db.reads[i].coff = -1;
db.allarr &= ~DB_ARROW;
index_name = Strdup(Catenate(pwd,PATHSEP,root,".idx"),"Allocating index file name");
index = Fopen(index_name,"w");
if (index_name == NULL || index == NULL)
exit (1);
FFWRITE(&db,sizeof(DAZZ_DB),1,index)
FFWRITE(db.reads,sizeof(DAZZ_READ),db.nreads,index)
FCLOSE(index);
}
Close_DB(&db);
exit (0);
}
int main(int argc, char *argv[])
{ void *iblock, *fblock;
int64 isize, osize;
int64 ovlsize, ptrsize;
int tspace, tbytes;
int i;
int VERBOSE;
// Process options
{ int j, k;
int flags[128];
ARG_INIT("LAsort")
j = 1;
for (i = 1; i < argc; i++)
if (argv[i][0] == '-')
{ ARG_FLAGS("v") }
else
argv[j++] = argv[i];
argc = j;
VERBOSE = flags['v'];
if (argc <= 1)
{ fprintf(stderr,"Usage: %s %s\n",Prog_Name,Usage);
exit (1);
}
}
// For each file do
ptrsize = sizeof(void *);
ovlsize = sizeof(Overlap) - ptrsize;
isize = 0;
iblock = NULL;
osize = MEMORY * 1000000ll;
fblock = Malloc(osize,"Allocating LAsort output block");
for (i = 1; i < argc; i++)
{ int64 *perm;
FILE *input, *foutput;
int64 novl;
// Read in the entire file and output header
{ int64 size;
struct stat info;
char *pwd, *root, *name;
pwd = PathTo(argv[i]);
root = Root(argv[i],".las");
name = Catenate(pwd,"/",root,".las");
input = Fopen(name,"r");
if (input == NULL)
exit (1);
stat(name,&info);
size = info.st_size;
fread(&novl,sizeof(int64),1,input);
fread(&tspace,sizeof(int),1,input);
if (tspace <= TRACE_XOVR)
tbytes = sizeof(uint8);
else
tbytes = sizeof(uint16);
if (VERBOSE)
{ printf(" %s: ",root);
Print_Number(novl,0,stdout);
printf(" records ");
Print_Number(size-novl*ovlsize,0,stdout);
printf(" trace bytes\n");
fflush(stdout);
}
foutput = Fopen(Catenate(pwd,"/",root,".S.las"),"w");
if (foutput == NULL)
exit (1);
fwrite(&novl,sizeof(int64),1,foutput);
fwrite(&tspace,sizeof(int),1,foutput);
free(pwd);
free(root);
if (size > isize)
{ if (iblock == NULL)
iblock = Malloc(size+ptrsize,"Allocating LAsort input block");
else
iblock = Realloc(iblock-ptrsize,size+ptrsize,"Allocating LAsort input block");
if (iblock == NULL)
exit (1);
iblock += ptrsize;
isize = size;
}
fread(iblock,1,isize,input);
fclose(input);
}
// Set up unsorted permutation array
perm = (int64 *) Malloc(sizeof(int64)*novl,"Allocating LAsort permutation vector");
if (perm == NULL)
exit (1);
{ int64 off;
int j;
off = -ptrsize;
for (j = 0; j < novl; j++)
{ perm[j] = off;
off += ovlsize + ((Overlap *) (iblock+off))->path.tlen*tbytes;
}
}
// Sort permutation array of ptrs to records
IBLOCK = iblock;
qsort(perm,novl,sizeof(int64),SORT_OVL);
// Output the records in sorted order
{ int j;
Overlap *w;
int64 tsize, span;
void *fptr, *ftop;
fptr = fblock;
ftop = fblock + osize;
for (j = 0; j < novl; j++)
{ w = (Overlap *) (iblock+perm[j]);
tsize = w->path.tlen*tbytes;
span = ovlsize + tsize;
if (fptr + span > ftop)
{ fwrite(fblock,1,fptr-fblock,foutput);
fptr = fblock;
}
memcpy(fptr,((void *) w)+ptrsize,ovlsize);
fptr += ovlsize;
memcpy(fptr,(void *) (w+1),tsize);
fptr += tsize;
}
if (fptr > fblock)
fwrite(fblock,1,fptr-fblock,foutput);
}
free(perm);
fclose(foutput);
}
if (iblock != NULL)
free(iblock - ptrsize);
free(fblock);
exit (0);
}
VON = flags['v'];
BON = flags['b'];
DON = flags['d'];
if (argc < 2 || argc > 3)
{ fprintf(stderr,"Usage: %s %s\n",Prog_Name,Usage[0]);
fprintf(stderr," %*s %s\n",(int) strlen(Prog_Name),"",Usage[1]);
fprintf(stderr," %*s %s\n",(int) strlen(Prog_Name),"",Usage[2]);
fprintf(stderr," %*s %s\n",(int) strlen(Prog_Name),"",Usage[3]);
exit (1);
}
}
// Make sure DB exists and is partitioned, get number of blocks in partition
pwd = PathTo(argv[1]);
root = Root(argv[1],".db");
{ int i, nfiles;
FILE *dbvis;
dbvis = Fopen(Catenate(pwd,"/",root,".db"),"r");
if (dbvis == NULL)
exit (1);
fscanf(dbvis,"files = %d\n",&nfiles);
for (i = 0; i < nfiles; i++)
{ char buffer[30001];
fgets(buffer,30000,dbvis);
}
int main(int argc, char* argv[])
{ int VERBOSE;
int KEEP;
int LOSSY;
{ int i, j, k;
int flags[128];
ARG_INIT("dexqv")
j = 1;
for (i = 1; i < argc; i++)
if (argv[i][0] == '-')
{ ARG_FLAGS("vkl") }
else
argv[j++] = argv[i];
argc = j;
VERBOSE = flags['v'];
KEEP = flags['k'];
LOSSY = flags['l'];
if (argc == 1)
{ fprintf(stderr,"Usage: %s %s\n",Prog_Name,Usage);
exit (1);
}
}
// For each .quiva file to be compressed:
{ int i;
for (i = 1; i < argc; i++)
{ char *pwd, *root;
FILE *input, *output;
QVcoding *coding;
pwd = PathTo(argv[i]);
root = Root(argv[i],".quiva");
input = Fopen(Catenate(pwd,"/",root,".quiva"),"r");
if (input == NULL)
exit (1);
output = Fopen(Catenate(pwd,"/",root,".dexqv"),"w");
if (output == NULL)
exit (1);
if (VERBOSE)
{ fprintf(stderr,"Processing '%s' ...\n",root);
fflush(stderr);
}
// Scan the file collecting statistics for Huffman schemes
QVcoding_Scan(input);
// Create and output the encoding schemes
coding = Create_QVcoding(LOSSY);
{ char *slash, *read;
rewind (input);
Read_Lines(input,1);
read = QVentry();
slash = index(read+1,'/');
if (slash != NULL)
{ coding->prefix = (char *) malloc((slash-read)+1);
if (coding->prefix == NULL)
{ fprintf(stderr,"%s: Out of memory (Allocating header prefix)\n",Prog_Name);
exit (1);
}
*slash = '\0';
if (strcpy(coding->prefix,read) == NULL)
SYSTEM_ERROR
*slash = '/';
}
}
Write_QVcoding(output,coding);
// For each entry do
{ int lwell;
rewind (input);
lwell = 0;
while (Read_Lines(input,1) > 0)
{ int well, beg, end, qv;
char *slash;
uint16 half;
uint8 byte;
// Interpret the header, encode and write out the fields
slash = index(QVentry(),'/');
sscanf(slash+1,"%d/%d_%d RQ=0.%d\n",&well,&beg,&end,&qv);
while (well - lwell >= 255)
{ byte = 0xff;
fwrite(&byte,1,1,output);
lwell += 255;
}
byte = (uint8) (well-lwell);
fwrite(&byte,1,1,output);
lwell = well;
half = (uint16) beg;
fwrite(&half,sizeof(uint16),1,output);
half = (uint16) end;
fwrite(&half,sizeof(uint16),1,output);
half = (uint16) qv;
fwrite(&half,sizeof(uint16),1,output);
Compress_Next_QVentry(input,output,coding,LOSSY);
}
}
// Clean up for the next file
Free_QVcoding(coding);
fclose(input);
fclose(output);
if (!KEEP)
unlink(Catenate(pwd,"/",root,".quiva"));
free(root);
free(pwd);
if (VERBOSE)
{ fprintf(stderr,"Done\n");
fflush(stderr);
}
}
}
free(QVentry());
exit (0);
}
int main(int argc, char *argv[])
{ HITS_DB _db, *db = &_db;
FILE *dbfile, *quiva;
int VERBOSE, UPPER;
// Process arguments
{ int i, j, k;
int flags[128];
ARG_INIT("DB2quiva")
j = 1;
for (i = 1; i < argc; i++)
if (argv[i][0] == '-')
{ ARG_FLAGS("vU") }
else
argv[j++] = argv[i];
argc = j;
VERBOSE = flags['v'];
UPPER = flags['U'];
if (argc != 2)
{ fprintf(stderr,"Usage: %s %s\n",Prog_Name,Usage);
exit (1);
}
}
// Open db, db stub file, and .qvs file
{ char *pwd, *root;
if (Open_DB(argv[1],db))
{ fprintf(stderr,"%s: Database %s.db could not be opened\n",Prog_Name,argv[1]);
exit (1);
}
if (db->part > 0)
{ fprintf(stderr,"%s: Cannot be called on a block: %s.db\n",Prog_Name,argv[1]);
exit (1);
}
pwd = PathTo(argv[1]);
root = Root(argv[1],".db");
dbfile = Fopen(Catenate(pwd,"/",root,".db"),"r");
quiva = Fopen(Catenate(pwd,PATHSEP,root,".qvs"),"r");
free(pwd);
free(root);
if (dbfile == NULL || quiva == NULL)
exit (1);
}
// For each file do:
{ HITS_READ *reads;
int f, first, nfiles;
QVcoding *coding;
char **entry;
if (fscanf(dbfile,DB_NFILE,&nfiles) != 1)
SYSTEM_ERROR
entry = New_QV_Buffer(db);
reads = db->reads;
first = 0;
for (f = 0; f < nfiles; f++)
{ int i, last;
FILE *ofile;
char prolog[MAX_NAME], fname[MAX_NAME];
// Scan db image file line, create .quiva file for writing
if (reads[first].coff < 0) break;
if (fscanf(dbfile,DB_FDATA,&last,fname,prolog) != 3)
SYSTEM_ERROR
if ((ofile = Fopen(Catenate(".","/",fname,".quiva"),"w")) == NULL)
exit (1);
if (VERBOSE)
{ fprintf(stderr,"Creating %s.quiva ...\n",fname);
fflush(stderr);
}
coding = Read_QVcoding(quiva);
// For the relevant range of reads, write the header for each to the file
// and then uncompress and write the quiva entry for each
for (i = first; i < last; i++)
{ int e, flags, qv, rlen;
HITS_READ *r;
r = reads + i;
flags = r->flags;
rlen = r->end - r->beg;
qv = (flags & DB_QV);
fprintf(ofile,"@%s/%d/%d_%d",prolog,r->origin,r->beg,r->end);
if (qv > 0)
fprintf(ofile," RQ=0.%3d",qv);
fprintf(ofile,"\n");
Uncompress_Next_QVentry(quiva,entry,coding,rlen);
if (UPPER)
{ char *deltag = entry[1];
int j;
for (j = 0; j < rlen; j++)
deltag[j] -= 32;
}
for (e = 0; e < 5; e++)
fprintf(ofile,"%.*s\n",rlen,entry[e]);
}
first = last;
}
}
fclose(quiva);
fclose(dbfile);
Close_DB(db);
exit (0);
}
