int main(void) { stldata newgun; stldata part; if(loadstl("robot_stl/newgun.STL", &newgun) != 0) { printf("load newgun stl error\n"); } if(loadstl("robot_stl/J6.STL", &part) != 0) { printf("load part stl error!\n"); } printf("stlmodel1: %s, %d triangles\n", newgun.modelname, newgun.num); printf("stlmodel2: %s, %d tirangles\n", part.modelname, part.num); printf("starting collisiondetection...\n"); long n1, n2; n1 = 0; n2 = 0; for(int i = 0; i < newgun.num; i++) for(int j = 0; j < part.num; j++) trianglecollisiondetection(newgun.ptriangle[i], part.ptriangle[j], n1, n2); printf("there are %ld potential collision triangle\n, n1 = %d, n2 = %d\n", n1 + n2, n1, n2); free(newgun.ptriangle); free(part.ptriangle); return 0; }
int main(int argc, char *argv[]) { stldata stl; if (loadstl(argv[1], &stl) != 0) { printf("loadstl %s error!\n", argv[1]); exit(1); } printf("triangle number = %d\n", stl.num); int *n = (int *)malloc(sizeof(int)); int *index = (int *)malloc(sizeof(int)); triangle *tmp = (triangle *)malloc(sizeof(triangle) * 2 * stl.num); triangle **array = (triangle **)malloc(sizeof(triangle *)); *n = stl.num; *index = 0; *array = tmp; int i; for (i = 0; i < stl.num; ++i) { triangle_cut_recur(array, n, index, &stl.ptriangle[i]); } for (i = 0; i < *index; ++i) show_triangle(*array, i); return 0; }
int main(int argc, char *argv[]) { FILE *fp; char comment[80]; float *verts; vertex_t *tris; halfedge_t *enext; vertex_t *evert; halfedge_t *vnext; uint16_t *attrs; triangle_t ntris; halfedge_t nedges; vertex_t nverts; int i, nrounds, opt; nrounds = 5; while((opt = getopt(argc, argv, "n:")) != -1){ switch(opt){ case 'n': nrounds = strtol(optarg, NULL, 10); break; default: caseusage: fprintf(stderr, "usage: %s [-n nrounds] path/to/file.stl\n", argv[0]); exit(1); } } if(optind == argc) goto caseusage; for(i = 0; i < nrounds; i++){ if(argc > 1){ fp = fopen(argv[optind], "rb"); } else { fp = stdin; } loadstl(fp, comment, &verts, &nverts, &tris, &attrs, &ntris); halfedges(tris, ntris, &enext, &evert, &nedges); dualedges(enext, nedges, &vnext); fclose(fp); free(tris); free(verts); free(attrs); free(enext); free(evert); free(vnext); } return 0; }
int main(void) { stldata newgun; stldata part; stldata j0, j1, j2, j3, j4, j5, j6; if (loadstl("45.STL", &newgun) != 0) printf("load 45 stl error\n"); if (loadstl("qian.STL", &part) != 0) printf("load qian stl error!\n"); if (loadstl("robot_stl/J0.STL", &j0) != 0) printf("load newgun stl error\n"); if (loadstl("robot_stl/J1.STL", &j1) != 0) printf("load newgun stl error\n"); if (loadstl("robot_stl/J2.STL", &j2) != 0) printf("load newgun stl error\n"); if (loadstl("robot_stl/J3.STL", &j3) != 0) printf("load newgun stl error\n"); if (loadstl("robot_stl/J4.STL", &j4) != 0) printf("load newgun stl error\n"); if (loadstl("robot_stl/J5.STL", &j5) != 0) printf("load newgun stl error\n"); if (loadstl("robot_stl/J6.STL", &j6) != 0) printf("load newgun stl error\n"); printf("newgun: %s, %d triangles\n", newgun.modelname, newgun.num); printf("part: %s, %d tirangles\n", part.modelname, part.num); printf("j0: %s, %d triangles\n", j0.modelname, j0.num); printf("j1: %s, %d triangles\n", j1.modelname, j1.num); printf("j2: %s, %d triangles\n", j2.modelname, j2.num); printf("j3: %s, %d triangles\n", j3.modelname, j3.num); printf("j4: %s, %d triangles\n", j4.modelname, j4.num); printf("j5: %s, %d triangles\n", j5.modelname, j5.num); printf("j6: %s, %d triangles\n", j6.modelname, j6.num); JAngle robotangle(0.0, -25.00, 65.00, 0.00, 0.00, 0.00); JAngle exangle(0.00, 0.00, 0.00, 0.00, 0.00, 0.00); TRANS j0_trans = Transform::getTransWorldToBase(exangle); TRANS j1_trans, j2_trans, j3_trans, j4_trans, j5_trans, j6_trans; TRANS part_trans, newgun_trans; Transform::getTransBaseToJoints(robotangle, j1_trans, j2_trans, j3_trans, j4_trans, j5_trans, j6_trans); newgun_trans = Transform::getTrans6ToGun(); j1_trans = j0_trans * j1_trans; j2_trans = j0_trans * j2_trans; j3_trans = j0_trans * j3_trans; j4_trans = j0_trans * j4_trans; j5_trans = j0_trans * j5_trans; j6_trans = j0_trans * j6_trans; newgun_trans = j6_trans * newgun_trans; part_trans = Transform::getTransWorldToWorkpiece(exangle); stldata newgunbuffer, partbuffer, j0buffer, j1buffer, j2buffer, j3buffer, j4buffer, j5buffer, j6buffer; strcpy(newgunbuffer.modelname, newgun.modelname); strcpy(partbuffer.modelname, part.modelname); strcpy(j0buffer.modelname, j0.modelname); strcpy(j1buffer.modelname, j1.modelname); strcpy(j2buffer.modelname, j2.modelname); strcpy(j3buffer.modelname, j3.modelname); strcpy(j4buffer.modelname, j4.modelname); strcpy(j5buffer.modelname, j5.modelname); strcpy(j6buffer.modelname, j6.modelname); newgunbuffer.num = newgun.num; partbuffer.num = part.num; j0buffer.num = j0.num; j1buffer.num = j1.num; j2buffer.num = j2.num; j3buffer.num = j3.num; j4buffer.num = j4.num; j5buffer.num = j5.num; j6buffer.num = j6.num; newgunbuffer.ptriangle = (triangle *)malloc(newgun.num * sizeof(triangle)); partbuffer.ptriangle = (triangle *)malloc(part.num * sizeof(triangle)); j0buffer.ptriangle = (triangle *)malloc(j0.num * sizeof(triangle)); j1buffer.ptriangle = (triangle *)malloc(j1.num * sizeof(triangle)); j2buffer.ptriangle = (triangle *)malloc(j2.num * sizeof(triangle)); j3buffer.ptriangle = (triangle *)malloc(j3.num * sizeof(triangle)); j4buffer.ptriangle = (triangle *)malloc(j4.num * sizeof(triangle)); j5buffer.ptriangle = (triangle *)malloc(j5.num * sizeof(triangle)); j6buffer.ptriangle = (triangle *)malloc(j6.num * sizeof(triangle)); /* newgun_trans.rot.mem[0][0] = -0.31028574744121573; */ /* newgun_trans.rot.mem[0][1] = -0.76055620150283443; */ /* newgun_trans.rot.mem[0][2] = 0.57033062278859459; */ /* newgun_trans.rot.mem[1][0] = -0.20326345994589520; */ /* newgun_trans.rot.mem[1][1] = 0.63914569443991687; */ /* newgun_trans.rot.mem[1][2] = 0.74173900202816612; */ /* newgun_trans.rot.mem[2][0] = -0.92865855985161161; */ /* newgun_trans.rot.mem[2][1] = 0.11422366494950395; */ /* newgun_trans.rot.mem[2][2] = -0.35291108452389802; */ /* newgun_trans.pos.dx = 94.771302128496586; */ /* newgun_trans.pos.dy = -505.20287976357167; */ /* newgun_trans.pos.dz = 1222.0426991063437; */ newgun_trans.rot.mem[0][0] = -0.036801385144649992; newgun_trans.rot.mem[0][1] = -0.74802231932451657; newgun_trans.rot.mem[0][2] = 0.66265244875711904; newgun_trans.rot.mem[1][0] = 0.73624785273584337; newgun_trans.rot.mem[1][1] = 0.42807339281869838; newgun_trans.rot.mem[1][2] = 0.52411093263024766; newgun_trans.rot.mem[2][0] = -0.67571055740849195; newgun_trans.rot.mem[2][1] = 0.50716445079782491; newgun_trans.rot.mem[2][2] = 0.53497613260186283; newgun_trans.pos.dx = 159.55751427696521; newgun_trans.pos.dy = 700.79742899056282; newgun_trans.pos.dz = 2790.4503015644741; part_trans.rot.mem[0][0] = -5.1036340138841535e-012; part_trans.rot.mem[0][1] = 1.0000000000000000; part_trans.rot.mem[0][2] = -5.1036340138841535e-012; part_trans.rot.mem[1][0] = -1.0000000000000000; part_trans.rot.mem[1][1] = -5.1036340138581062e-012; part_trans.rot.mem[1][2] = -5.1036340139102000e-012; part_trans.rot.mem[2][0] = -5.1036340138841527e-012; part_trans.rot.mem[2][1] = -5.1036340139102008e-012; part_trans.rot.mem[2][2] = 1.0000000000000000; part_trans.pos.dx = -600.0000000000; part_trans.pos.dy = 429.99999998934874; part_trans.pos.dz = 2087.0000000021946; tran(&newgun_trans, &newgun, &newgunbuffer); tran(&part_trans, &part, &partbuffer); tran(&j0_trans, &j0, &j0buffer); tran(&j1_trans, &j1, &j1buffer); tran(&j2_trans, &j2, &j2buffer); tran(&j3_trans, &j3, &j3buffer); tran(&j4_trans, &j4, &j4buffer); tran(&j5_trans, &j5, &j5buffer); tran(&j6_trans, &j6, &j6buffer); if (writestl("mystl/45.STL", &newgunbuffer)) printf("stl 45.STL write error!\n"); if (writestl("mystl/qian.STL", &partbuffer)) printf("stl part write error!\n"); if (writestl("mystl/j0.STL", &j0buffer)) printf("stl j0 write error!\n"); if (writestl("mystl/j1.STL", &j1buffer)) printf("stl j1 write error!\n"); if (writestl("mystl/j2.STL", &j2buffer)) printf("stl j2 write error!\n"); if (writestl("mystl/j3.STL", &j3buffer)) printf("stl j3 write error!\n"); if (writestl("mystl/j4.STL", &j4buffer)) printf("stl j4 write error!\n"); if (writestl("mystl/j5.STL", &j5buffer)) printf("stl j5 write error!\n"); if (writestl("mystl/j6.STL", &j6buffer)) printf("stl j6 write error!\n"); /* for (i = 0; i < newgun.num; ++i) { */ /* printf("Point %d: (%f, %f, %f), (%f, %f, %f), (%f, %f, %f), (%f, %f, %f)\n", */ /* i, newgun.ptriangle[i].normalvector.x, newgun.ptriangle[i].normalvector.y, */ /* newgun.ptriangle[i].normalvector.z, newgun.ptriangle[i].vertex1.x, */ /* newgun.ptriangle[i].vertex1.y, newgun.ptriangle[i].vertex1.z, */ /* newgun.ptriangle[i].vertex2.x, newgun.ptriangle[i].vertex2.y, */ /* newgun.ptriangle[i].vertex2.z, newgun.ptriangle[i].vertex3.x, */ /* newgun.ptriangle[i].vertex3.y, newgun.ptriangle[i].vertex3.z); */ /* } */ /* for (i = 0; i < newgun.num; ++i) { */ /* printf("distance %d: %f %f\n", i, distance(&newgun.ptriangle[i].vertex1, &newgun.ptriangle[i].vertex2), distance(&newgunbuffer.ptriangle[i].vertex1, &newgunbuffer.ptriangle[i].vertex2)); */ /* } */ /* printf("part\n"); */ /* for (i = 0; i < part.num; ++i) { */ /* printf("distance %d: %f %f\n", i, distance(&part.ptriangle[i].vertex1, &part.ptriangle[i].vertex2), distance(&partbuffer.ptriangle[i].vertex1, &partbuffer.ptriangle[i].vertex2)); */ /* } */ free(newgun.ptriangle); free(part.ptriangle); free(j0.ptriangle); free(j1.ptriangle); free(j2.ptriangle); free(j3.ptriangle); free(j4.ptriangle); free(j5.ptriangle); free(j6.ptriangle); free(newgunbuffer.ptriangle); free(partbuffer.ptriangle); free(j0buffer.ptriangle); free(j1buffer.ptriangle); free(j2buffer.ptriangle); free(j3buffer.ptriangle); free(j4buffer.ptriangle); free(j5buffer.ptriangle); free(j6buffer.ptriangle); return 0; }
int main(void) { stldata newgun; stldata part; if (loadstl("robot_stl/newgun.STL", &newgun) != 0){ printf("load newgun stl error\n"); exit(1); } if (loadstl("robot_stl/Part.STL", &part) != 0) { printf("load part stl error!\n"); exit(1); } printf("stlmodel1: %s, %d triangles\n", newgun.modelname, newgun.num); printf("stlmodel2: %s, %d tirangles\n", part.modelname, part.num); printf("starting collisiondetection...\n"); JAngle robotangle(0.0, -30.00, 72.00, 0.00, 0.00, 0.00); JAngle exangle(0.00, 0.00, 0.00, 0.00, 0.00, 0.00); TRANS j0_trans = Transform::getTransWorldToBase(exangle); TRANS j1_trans, j2_trans, j3_trans, j4_trans, j5_trans, j6_trans; TRANS workpiece_trans, gun_trans; Transform::getTransBaseToJoints(robotangle, j1_trans, j2_trans, j3_trans, j4_trans, j5_trans, j6_trans); j1_trans = j0_trans * j1_trans; j2_trans = j0_trans * j2_trans; j3_trans = j0_trans * j3_trans; j4_trans = j0_trans * j4_trans; j5_trans = j0_trans * j5_trans; j6_trans = j0_trans * j6_trans; gun_trans = j6_trans * Transform::getTrans6ToGun(); workpiece_trans = Transform::getTransWorldToWorkpiece(exangle); int i, j; triangle ttri1, ttri2; vector3d tvec1, tvec2; int collision = 0; for (i = 0; i < part.num; ++i) { tvec1.x = workpiece_trans.pos.dx; tvec1.y = workpiece_trans.pos.dy; tvec1.z = workpiece_trans.pos.dz; coordinatetransform(workpiece_trans.rot.mem, &tvec1, &part.ptriangle[i], &ttri1); for (j = 0; j < newgun.num; ++j) { tvec2.x = gun_trans.pos.dx; tvec2.y = gun_trans.pos.dy; tvec2.z = gun_trans.pos.dz; coordinatetransform(gun_trans.rot.mem, &tvec2, &newgun.ptriangle[j], &ttri2); if (triangleCD(&ttri1, &ttri2)) { collision = 1; break; } } if (collision == 1) break; } if (collision == 1) { printf("collision detected!\n"); } else { printf("no collision\n"); } free(newgun.ptriangle); free(part.ptriangle); return 0; }