struct gadget_particle_t *read_gadget_particles(char *file_name)
{
FILE *fp;
if (!(fp = fopen(file_name, "r"))) {
fprintf(stderr, "Cannot open %s\n", file_name);
exit(1);
}
struct gadget_header_t *header = malloc(sizeof(*header));
check_mem(header);
int dummy;
fread(&dummy, sizeof(dummy), 1, fp);
fread(header, sizeof(*header), 1, fp);
fread(&dummy, sizeof(dummy), 1, fp);
uint32_t len = header->npart[1];
struct gadget_particle_t *particles = malloc(sizeof(*particles) * len);
void *buf = malloc(sizeof(float) * 3 * len);
check_mem(buf);
check_mem(particles);
read_positions(fp, header, buf, particles);
read_velocities(fp, header, buf, particles);
read_ids(fp, header, buf, particles);
free(header);
free(buf);
fclose(fp);
return particles;
}
int main() {
std::vector<tutor::PPoint2d>u1;
std::vector<tutor::PPoint2d>u2 ;
char* file_names[] = {"imgs/img12.txt", "imgs/img22.txt"};
char* img_names[] = {"imgs/image1.pgm", "imgs/image2.pgm"};
///// get points
u1 = read_positions(file_names[0]);
u2 = read_positions(file_names[1]);
// std::cout << "u1=" << u1 << std::endl;
// std::cout << "u2=" << u2 << std::endl;
///// get images
cv::Mat img1 = cv::imread(img_names[0]);
cv::Mat img2 = cv::imread(img_names[1]);
Drawer drawer = Drawer();
for (unsigned i=0; i<N; i++) {
drawer.mark(img1, u1[i], 255);
drawer.mark(img2, u2[i], 255);
}
///// get F
cv::Matx33d F ;
F = get_fundamental_matrix(u1, u2);
///// draw epipoler line
for (unsigned i=0; i<u1.size(); i++) {
// PPoint2d u = pt_to_ppt(u1[i]);
std::cout << "i=" << i << " " ;
draw_epiline(img1, u1[i], F) ;
}
for (unsigned i=0; i<u2.size(); i++) {
// PPoint2d u = pt_to_ppt(u2[i]);
draw_epiline(img2, u2[i], F.t()) ;
}
cv::imwrite("image1.jpg", img1);
cv::imwrite("image2.jpg", img2);
return 0;
}
int main(int argc, char *argv[])
{
/*---Arguments-------------------------*/
double boxsize;
char *file=NULL,*fileformat=NULL;
char *binfile=NULL;
DOUBLE pimax ;
/*---Data-variables--------------------*/
int64_t ND1=0;
DOUBLE *x1=NULL,*y1=NULL,*z1=NULL;
/*---Corrfunc-variables----------------*/
#if !(defined(USE_OMP) && defined(_OPENMP))
const char argnames[][30]={"boxsize","file","format","binfile","pimax"};
#else
int nthreads=2;
const char argnames[][30]={"boxsize","file","format","binfile","pimax","Nthreads"};
#endif
int nargs=sizeof(argnames)/(sizeof(char)*30);
struct timeval t_end,t_start,t0,t1;
double read_time=0.0;
gettimeofday(&t_start,NULL);
/*---Read-arguments-----------------------------------*/
if(argc< (nargs+1)) {
Printhelp() ;
fprintf(stderr,"\nFound: %d parameters\n ",argc-1);
int i;
for(i=1;i<argc;i++) {
if(i <= nargs)
fprintf(stderr,"\t\t %s = `%s' \n",argnames[i-1],argv[i]);
else
fprintf(stderr,"\t\t <> = `%s' \n",argv[i]);
}
if(i <= nargs) {
fprintf(stderr,"\nMissing required parameters \n");
for(i=argc;i<=nargs;i++)
fprintf(stderr,"\t\t %s = `?'\n",argnames[i-1]);
}
return EXIT_FAILURE;
}
boxsize=atof(argv[1]);
file=argv[2];
fileformat=argv[3];
binfile=argv[4];
pimax=40.0;
#ifdef DOUBLE_PREC
sscanf(argv[5],"%lf",&pimax) ;
#else
sscanf(argv[5],"%f",&pimax) ;
#endif
#if defined(USE_OMP) && defined(_OPENMP)
nthreads=atoi(argv[6]);
assert(nthreads >= 1 && "Number of threads must be at least 1");
#endif
fprintf(stderr,"Running `%s' with the parameters \n",argv[0]);
fprintf(stderr,"\n\t\t -------------------------------------\n");
for(int i=1;i<argc;i++) {
if(i <= nargs) {
fprintf(stderr,"\t\t %-10s = %s \n",argnames[i-1],argv[i]);
} else {
fprintf(stderr,"\t\t <> = `%s' \n",argv[i]);
}
}
fprintf(stderr,"\t\t -------------------------------------\n");
gettimeofday(&t0,NULL);
/*---Read-data1-file----------------------------------*/
ND1=read_positions(file,fileformat,sizeof(DOUBLE), 3, &x1, &y1, &z1);
gettimeofday(&t1,NULL);
read_time += ADD_DIFF_TIME(t0,t1);
//check that theee positions are within limits
for(int i=0;i<ND1;i++) {
assert(x1[i] >= 0.0 && x1[i] <= boxsize && "xpos is within limits [0, boxsize]");
assert(y1[i] >= 0.0 && y1[i] <= boxsize && "ypos is within limits [0, boxsize]");
assert(z1[i] >= 0.0 && z1[i] <= boxsize && "zpos is within limits [0, boxsize]");
}
/*---Count-pairs--------------------------------------*/
gettimeofday(&t0,NULL);
results_countpairs_wp *results = countpairs_wp(ND1, x1, y1, z1,
boxsize,
#if defined(USE_OMP) && defined(_OPENMP)
nthreads,
#endif
binfile,
pimax);
gettimeofday(&t1,NULL);
double pair_time = ADD_DIFF_TIME(t0,t1);
free(x1);free(y1);free(z1);
//Output the results
/* Note: we discard the first bin, to mimic the fact that close pairs
* are disregarded in SDSS data.
*/
DOUBLE rlow=results->rupp[0];
for(int i=1;i<results->nbin;++i) {
fprintf(stdout,"%e\t%e\t%e\t%e\t%12"PRIu64" \n",results->wp[i],results->rpavg[i],rlow,results->rupp[i],results->npairs[i]);
rlow=results->rupp[i];
}
//free the memory in the results struct
free_results_wp(&results);
gettimeofday(&t_end,NULL);
fprintf(stderr,"wp> Done - ND1=%12"PRId64". Time taken = %6.2lf seconds. read-in time = %6.2lf seconds pair-counting time = %6.2lf sec\n",
ND1,ADD_DIFF_TIME(t_start,t_end),read_time,pair_time);
return EXIT_SUCCESS;
}
int main(int argc, char *argv[])
{
int ret;
int choice;
int i, j;
float Ts=0.002;
float T;
float vm=0.05; //mean speed
matrix xd, xd_dot, J;
float q0[5], q_final[5];
float p0[3];
float p1[3];
DUNIT *unit;
unsigned short motor[NUM_MOTORS];
unsigned char sensor[NUM_SENSORS];
pSM = (SM*) mbuff_alloc(NAME_OF_MEMORY, sizeof(SM));
if(pSM == NULL) {
perror("mbuff_alloc failed");
return -1;
}
// Header = 1,;
// Step = 2,
motor[0] = 0;
motor[1] = 0;
motor[2] = 2100;
motor[3] = 4200;
motor[4] = -2500;
motor[5] = 0;
motor[6] = 18100;
motor[7] = -2100;
motor[8] = 1000;
motor[9] = 6200;
motor[10] = 0;
motor[11] = 0;
motor[12] = -2100;
motor[13] = -4180;
motor[14] = 2500;
motor[15] = 0;
motor[16] = -18810;
motor[17] = 2000;
motor[18] = -1000;
motor[19] = -6200;
motor[20] = 2100;
motor[21] = 60;
motor[22] = 60;
sensor[0] = 'R';
sensor[1] = 'R';
sensor[2] = 'R';
sensor[3] = 'R';
unit = (DUNIT*)&(pSM->Data.IntDat);
ret = send_commands(motor, sensor, unit);
pSM->VarIF.ResRep = 0;
pSM->VarIF.Mode = IDLE;
pSM->VarIF.InterruptSend = TRUE;
//watch return
readret(unit);
//desired final configuration
//q_final[0]=PI*3/5;
//q_final[1]=PI/3;
//q_final[2]=PI/6;
//q_final[3]=-PI/4;
//q_final[4]=0;
//evaluate_position(q_final, p1);
q0[4]=0;
open_hand (motor, sensor, unit);
while (1)
{
//present position
read_positions(unit, motor);
robot2DH (&motor[16], q0);
evaluate_position(q0, p0);
printf("\nPresent position: (%f,%f,%f)\n",p0[0],p0[1],p0[2]);
//select desired position
choice=select_desired_position (p1);
if (choice==-1) break;
if (choice==1) close_hand (motor, sensor, unit);
if (choice==2) open_hand (motor, sensor, unit);
printf("\nDesired final position: (%f,%f,%f)\n",p1[0],p1[1],p1[2]);
//execution time
T=evaluate_execution_time (p0, p1, vm);
//initializing matrixes
initialize_matrix (&xd, T/Ts+1, 3);
initialize_matrix (&xd_dot, T/Ts+1, 3);
initialize_matrix (&J, 3, 5);
//evaluate desired trajectory
trajectory_left_arm (Ts, T, p0, p1, &xd, &xd_dot);
//inverse kinematics
inverse_kinematics (Ts, T, &xd, &xd_dot, unit);
}
//close_hand (motor, sensor, unit);
free(xd.vector);
free(xd_dot.vector);
free(J.vector);
//free memory
mbuff_free(NAME_OF_MEMORY, (void*)pSM);
return 0;
}
