/* nbody.c : This file is partly based on the original file from I_COLLIDE library distribution, but most of the functions are written for our vogl graphics library and simulatioin need. */ /*----------------------------- Local Includes -----------------------------*/ #include #include #include #include #include #include #include #include #include #include "/cs1/shared/robotics/toolkit/I_COLLIDE/newpro/polytope.h" /*----------------------------- Local Constants -----------------------------*/ #if 0 #define PRINT_COLLISIONS #endif /*------------------------------ Local Macros -------------------------------*/ #define ABS(a) (((a) > 0.0) ? (a) : (-a)) /*------------------------------- Local Types -------------------------------*/ /*------------------------ Local Function Prototypes ------------------------*/ int load_sim_poly(char *fname, Status *status, Polytope **poly); void vogl_draw_scene(Status *status, float size, int wire); void draw_poly(__col_Polyhedron *p, int wire); void openwindow(); double get_collision_plane(Status *status, Polytope *poly, int obj1, int obj2, void **feat1, void **feat2, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2); void cal_vel_change(col_Mat3 Mat, col_Vect3 Delta_v, double frict, double restit); double vect_from_quat(q_type quat, q_vec_type vect); void get_col_matrix(col_Mat3 mat1, col_Mat3 mat2, int obj1, int obj2, col_Vect3 vector1, col_Vect3 vector2, col_Mat3 Inert1, col_Mat3 Inert2); int mat3Invert(col_Mat3 mat, col_Mat3 invmat); void mat3Sub(col_Mat3 a, col_Mat3 b, col_Mat3 c); void mat3Add(col_Mat3 a, col_Mat3 b, col_Mat3 c); /*------------------------------ Local Globals ------------------------------*/ Status status; Polytope *Poly; int frames=0; int num_objects=0; int total_collisions=0; double cube_bound; double MIN_DIST; int use_box; clock_t OLDTIME; extern __col_State state; extern __col_Polyhedron __col_polyhedronLibrary[1000]; /*---------------------------------Functions-------------------------------- */ /*****************************************************************************\ @ polygon_area() ----------------------------------------------------------------------------- description : Compute the signed area of the 2D projection of a face input : A face, and which two dimensions to use for the area computation output : area of the projection of the face notes : \*****************************************************************************/ double polygon_area(__col_Face *face, int axis1, int axis2) { double area; col_Fnode *feature; __col_Vertex *v1, *v2; area = 0.0; for (feature=face->verts, v1 = feature->f.v; feature; feature = feature->next, v1 = v2) { v2 = (feature->next) ? feature->next->f.v : face->verts->f.v; area += v1->coords[axis1]*v2->coords[axis2] - v1->coords[axis2]*v2->coords[axis1]; } area /= 2.0; return area; } /** End of polygon_area() **/ /*****************************************************************************\ @ face_area() ----------------------------------------------------------------------------- description : Compute the unsigned area of a face input : output : notes : \*****************************************************************************/ double face_area(__col_Face *face) { double area, xy_area, yz_area, xz_area; /* project the polygon onto the three coordinate planes and calculate its area there */ xy_area = polygon_area(face, 0, 1); yz_area = polygon_area(face, 1, 2); xz_area = polygon_area(face, 0, 2); /* use the projected areas to calculate the area of the face */ area = sqrt(xy_area * xy_area + yz_area * yz_area + xz_area * xz_area); return area; } /** End of face_area **/ /*****************************************************************************\ @ polytope_volume() ----------------------------------------------------------------------------- description : Compute the volume of a polyhedron input : output : notes : \*****************************************************************************/ double polytope_volume(__col_Polyhedron *poly) { double volume, area; col_Fnode *feature; volume = 0.0; for(feature=poly->faces; feature; feature = feature->next) { area = face_area(feature->f.f); volume -= area * feature->f.f->plane[3]; } volume /= 3.0; return volume; } /** End of polytope_volume() **/ /*****************************************************************************\ @ load_sim_poly() ----------------------------------------------------------------------------- description : Open the simulation specification file and do initialization input : A file, status, pointer to poly output : notes : \*****************************************************************************/ int load_sim_poly(char *fname, Status *status, Polytope **poly) { FILE *fp, *fp1; char s[80]; char s1[80]; int n; int i = 0; int j1, j2, j; double initial_orientation[3]; double axis[3]; double dlt; if(!(fp = fopen(fname, "r"))) { fprintf(stderr, "Load simulation file failed.\n"); return -1; } if(!(fp1 = fopen("inert.dat", "r"))) { fprintf(stderr, "Load inertia file failed.\n"); return -1; } do fscanf(fp, "%s", s); while (!feof(fp) && strcmp(s, "start")); if (feof(fp)) return -1; fscanf(fp, "%d", &n); ALLOCATE(*poly, Polytope, n); while(1) { fscanf(fp, "%s", s); if (s[0] == '*') break; col_instance_polytope(s, &((*poly)[i].id)); (*poly)[i].volumn=polytope_volume(state.polytopes[i].polytope); do fscanf(fp1, "%s", s1); while (!feof(fp1) && strcmp(s, s1)); fscanf(fp1, "%lf %lf %lf ", &(*poly)[i].mass_center[X], &(*poly)[i].mass_center[Y], &(*poly)[i].mass_center[Z]); fscanf(fp1, "%lf %lf %lf ", &(*poly)[i].inertia[X][X], &(*poly)[i].inertia[X][Y], &(*poly)[i].inertia[X][Z]); fscanf(fp1, "%lf %lf %lf ", &(*poly)[i].inertia[Y][X], &(*poly)[i].inertia[Y][Y], &(*poly)[i].inertia[Y][Z]); fscanf(fp1, "%lf %lf %lf ", &(*poly)[i].inertia[Z][X], &(*poly)[i].inertia[Z][Y], &(*poly)[i].inertia[Z][Z]); rewind(fp1); fscanf(fp, "%d", &(*poly)[i].move); fscanf(fp, "%lf %lf %lf", &(*poly)[i].density, &(*poly)[i].friction, &(*poly)[i].restit); /* Code for handling fixed objects by increasing their density */ /* if((*poly)[i].move == 0) (*poly)[i].density = __COL_INFINITY; */ if((*poly)[i].move == 0) (*poly)[i].density = 1000000.0; for(j1=0;j1<3;j1++) for(j2=0;j2<3;j2++) (*poly)[i].inertia[j1][j2] = (*poly)[i].inertia[j1][j2]*(*poly)[i].density; fscanf(fp, "%lf %lf %lf ", &(*poly)[i].trans_tot[X], &(*poly)[i].trans_tot[Y], &(*poly)[i].trans_tot[Z]); fscanf(fp, "%lf %lf %lf", &(*poly)[i].trans_vel[X], &(*poly)[i].trans_vel[Y], &(*poly)[i].trans_vel[Z]); if((*poly)[i].move == 0) for(j=0;j<3;j++) (*poly)[i].trans_vel[j] = 0.0; fscanf(fp, "%lf %lf %lf", &initial_orientation[Z], &initial_orientation[Y], &initial_orientation[X]); q_from_euler((*poly)[i].tot_quat, Q_DEG_TO_RAD(initial_orientation[Z]), Q_DEG_TO_RAD(initial_orientation[Y]), Q_DEG_TO_RAD(initial_orientation[X])); fscanf(fp, "%lf %lf %lf %lf", &axis[X], &axis[Y], &axis[Z], &dlt); if((*poly)[i].move == 0) dlt = 0.0; q_make((*poly)[i].delta_quat, axis[X], axis[Y], axis[Z], dlt); i++; } fclose(fp); fclose(fp1); status->num_polytopes = i; for(i=0; inum_polytopes; i++) { col_calc_cuboid((*poly)[i].id, (*poly)[i].object_center, &(*poly)[i].max_radius); (*poly)[i].old_colliding = (*poly)[i].colliding = -1; /* dumpCones(state.polytopes[i].polytope); */ /* printf("poly:%d, mass center:%f %f %f\n", (*poly)[i].id, (*poly)[i].mass_center[X], (*poly)[i].mass_center[Y], (*poly)[i].mass_center[Z]); printf("mass inertia:%f %f %f\n", (*poly)[i].inertia[X][X], (*poly)[i].inertia[X][Y],(*poly)[i].inertia[X][Z]); printf("mass inertia:%f %f %f\n", (*poly)[i].inertia[Y][X], (*poly)[i].inertia[Y][Y],(*poly)[i].inertia[Y][Z]); printf("mass inertia:%f %f %f\n", (*poly)[i].inertia[Z][X], (*poly)[i].inertia[Z][Y],(*poly)[i].inertia[Z][Z]); printf("poly:%d, volume:%f density:%f\n", (*poly)[i].id, (*poly)[i].volumn, (*poly)[i].density); printf("poly:%d, frict:%f, restit:%f\n", (*poly)[i].id, (*poly)[i].friction, (*poly)[i].restit); printf("velocity:%f %f %f\n", (*poly)[i].trans_vel[X], (*poly)[i].trans_vel[Y],(*poly)[i].trans_vel[Z]); printf("angular:%f %f %f %f\n", (*poly)[i].delta_quat[X], (*poly)[i].delta_quat[Y],(*poly)[i].delta_quat[Z], (*poly)[i].delta_quat[W]); */ } return 1; } /*****************************************************************************\ @ mat_identity() ----------------------------------------------------------------------------- description : input : output : notes : \*****************************************************************************/ static void mat_identity(col_Mat4 m) { register int i,j; for (i = 0; i < 4; i++) for(j = 0; j < 4; j++) m[j][i] = (i==j) ? 1.0 : 0.0; } /** End of mat_identity() **/ /*****************************************************************************\ @ calc_translation() ----------------------------------------------------------------------------- description : input : output : notes : \*****************************************************************************/ static void calc_translation(col_Mat4 m, double x, double y, double z) { mat_identity(m); m[0][3] = x; m[1][3] = y; m[2][3] = z; } /** End of calc_translation() **/ /*****************************************************************************\ @ init_collision() ----------------------------------------------------------------------------- description : Read in the polytopes from a file. 1. Read in the object. 2. Pass the objects to collision library so it can create Voronoi cones. 3. Create initial positions for the objects. 4. Give initial velocities for the objects. 5. Give initial O(n^2) matrix the relationship between objects (bounce, stick, etc). input : output : notes : \*****************************************************************************/ void init_collision(Status *status, Polytope **poly, char *objFile, char *fname) { int i; printf("Loading library file: %s ...", objFile); fflush(stdout); if (col_open(DEFAULT_MAX_POLYTOPE, objFile, 1.0, &(status->num_lib_polytopes)) != COL_OK) { fprintf(stderr, "init_collision: couldn't open collision library\n"); exit(1); } printf("done.\n"); fflush(stdout); printf("load library finish with %d polytopes\n",status->num_lib_polytopes); if(load_sim_poly(fname, status, poly) == -1) { printf("Load simulation file failed!\n"); exit(1); } printf("load sim poly finish with %d polytopes\n",status->num_polytopes); /* different libcollide matrix allocation methods might have different effects on the random number generator, so seed the generator again after & initializing the library */ if (status->seed > 0) { srand(status->seed); } printf("Activating all pairs (slow for COL_SPARSE_MATRIX representation)..."); fflush(stdout); col_activate_all(); col_enable_nbody(); printf("done.\n"); fflush(stdout); if (status->use_cuboid) { printf("Using fixed size cube for bounding box\n"); fflush(stdout); for (i=0; i < status->num_polytopes; i++) { col_use_cuboid((*poly)[i].id); } } } /** End of init_collision() **/ /*****************************************************************************\ @ usage() ----------------------------------------------------------------------------- description : input : output : notes : \*****************************************************************************/ void usage() { printf("usage: nbody [-l ] [-f #frames] \n"); printf(" [-n ] [-s seed] [-b]\n"); printf(" [-v gravity] [-r time rate/tick] [-b]\n"); printf(" [-x use box in drawing scene] [-w box width]\n"); printf(" [-d correction factor]\n"); printf(" seed is an integer to seed the random number generator\n"); printf(" -b specifies using dynamic bounding boxes rather than\n"); printf(" fixed-sized bounding cubes\n"); printf("\n"); exit(1); } /** End of usage() **/ /*****************************************************************************\ @ initialize() ----------------------------------------------------------------------------- description : Control initialization. 1. Decide which file the polytopes will be read from. 2. Give initial signal to the collision library. 3. Read in polytopes. 4. Update State. state Updates the state of the simulation (whether do n-body and etc.). argc/argv Variables passed from the environment input : output : notes : \*****************************************************************************/ void initialize(int argc, char **argv, Status *status, Polytope **poly) { char objFile[256]; char fname[256]; extern char *optarg; int c; int args_found; status->seed = 0; status->use_cuboid = 1; strcpy(objFile, DEFAULT_LIBRARY); strcpy(fname, DEFAULT_SIM_POLY); status->num_frames = DEFAULT_FRAMES; status->gravity = DEFAULT_GRAVITY; status->correction = DEFAULT_CORRECTION; status->tick = DEFAULT_TICK; cube_bound = DEFAULT_CUBE_BOUND; use_box = DEFAULT_USE_BOX; args_found = 1; while ((c = getopt(argc, argv, "xXbBhHl:L:w:W:f:F:n:N:v:V:r:R:d:D:s:S:")) != EOF) /* Parse the various options. */ switch (c) { case 'l': case 'L': strcpy(objFile, optarg); args_found += 2; break; case 'n': case 'N': strcpy(fname, optarg); args_found += 2; break; case 'v': case 'V': status->gravity = atof(optarg); args_found += 2; break; case 'w': case 'W': cube_bound = atof(optarg); args_found += 2; break; case 'r': case 'R': status->tick = atof(optarg); args_found += 2; break; case 'd': case 'D': status->correction = atof(optarg); args_found += 2; break; case 'b': case 'B': status->use_cuboid = 0; args_found++; break; case 'x': case 'X': use_box = 1; args_found++; break; case 'f': case 'F': status->num_frames = atoi(optarg); args_found += 2; break; case 's': case 'S': status->seed = atoi(optarg); args_found += 2; break; case 'h': case 'H': args_found++; default: usage(); break; } if (args_found != argc) usage(); if (status->seed > 0) { srand(status->seed); printf("Random number seed: %d\n", status->seed); fflush(stdout); } init_collision(status, poly, objFile, fname); /* allocate space for reporting all n^2 possible collisions, there aren't likely to be anywhere near this many */ ALLOCATE(status->all_collisions, col_Report, status->num_polytopes * status->num_polytopes); ALLOCATE(status->all_dist, col_DistReport, status->num_polytopes * status->num_polytopes); openwindow(); } /** End of initialize() **/ /*****************************************************************************\ @ compute_transformation() ----------------------------------------------------------------------------- description : Compute a polytope's current matrix transformation from its total translation and rotation input : output : notes : \*****************************************************************************/ static void compute_transformation(Polytope *poly) { col_Mat4 xrot, yrot, zrot, trans; col_Mat4 trans_to_center, xrot_cent, zy, zyxt2c; col_Mat4 rot, rot_cent; calc_translation(trans_to_center, -poly->mass_center[X], -poly->mass_center[Y], -poly->mass_center[Z]); calc_translation(trans, poly->trans_tot[X] + poly->mass_center[X], poly->trans_tot[Y] + poly->mass_center[Y], poly->trans_tot[Z] + poly->mass_center[Z]); q_to_row_matrix(rot, poly->tot_quat); __col_matMultXform(rot, trans_to_center, rot_cent); __col_matMultXform(trans, rot_cent, poly->tot); } /** End of compute_transformation() **/ /*****************************************************************************\ @ compute_collision() ----------------------------------------------------------------------------- description : Compute all object transformations and perform collision test. Swap translations and rotations of colliding pairs input : output : notes : \*****************************************************************************/ void compute_collision(Status *status, Polytope *poly) { col_Mat4 T; double angle[3]; int i, j, obj1, obj2; double temp, tmp1, tmp2, dist; double delta_x, delta_y, delta_z; /* initial velocity diff. */ double delta_x_f, delta_y_f, delta_z_f; /* final velocity */ double frict, restit; /* friction & restitution coff */ q_type quat_temp; int dist_pair; void *feat1, *feat2; /* collidiing features */ col_Vect3 Delta_v, Old_Delta_v; /* velocity difference */ col_Vect3 impulse, impulse_inv, imp1, imp2; /* collision impulse */ col_Vect3 norm, Xaxis, Yaxis, pt1, pt2; /* collision plane & points */ col_Vect3 velo1, velo2, rel_velo, center1, center2; col_Vect3 vector1, vector2; col_Vect3 VECTOR1, VECTOR2; col_Vect3 Angul1, Angul2; col_Vect3 Delta_Ag1, Delta_Ag2; col_Mat3 Mat, Mat_inv, Mat1, Mat2; col_Mat3 Mt, Mt_inv; col_Mat3 Inert1, Inert2; /* inertia matrix */ q_vec_type angle_velo1, angle_velo2; clock_t start, finish; /* save previous status */ for (i=0; i < status->num_polytopes; i++) { poly[i].old_colliding = poly[i].colliding; poly[i].colliding = -1; } /* inform collision library of new object positions */ for (i= 0; i < status->num_polytopes; ++i) { compute_transformation(&(poly[i])); col_set_transform(poly[i].id, poly[i].tot); } /* perform collision test */ if (col_test(&MIN_DIST) != COL_OK) /* The biggie */ { fprintf(stderr, "col_test didn't return COL_OK\n"); exit(1); } /* inquire results of collision test */ status->num_collisions = col_report_collision_all(status->all_collisions, status->num_polytopes*status->num_polytopes); total_collisions += status->num_collisions; #ifdef PRINT_COLLISIONS if (status->num_collisions) printf("FRAME %d: %d collisions\n", frames, status->num_collisions); #endif /* exchange velocities of colliding objects, providing they weren't already colliding last frame (i.e. don't keep swapping until objects have seperated) */ for (i= 0; i < status->num_collisions; ++i) { obj1 = status->all_collisions[i].ids[0]; obj2 = status->all_collisions[i].ids[1]; #ifdef PRINT_COLLISIONS printf("ids: %d %d\n", obj1, obj2); #endif feat1 = status->all_collisions[i].features[0]; feat2 = status->all_collisions[i].features[1]; poly[obj1].colliding = obj2; poly[obj2].colliding = obj1; if ((poly[obj1].old_colliding != obj2) && (poly[obj2].old_colliding != obj1)) { /* Get the collision plane and coresponding colliding coordinates */ dist = get_collision_plane(status, Poly, obj1, obj2, &feat1, &feat2, norm, Xaxis, Yaxis, pt1, pt2); /* get the angular velocities */ vect_from_quat(poly[obj1].delta_quat, angle_velo1); vect_from_quat(poly[obj2].delta_quat, angle_velo2); /* calculate the center of mass and the colliding points position */ center1[X] = poly[obj1].mass_center[X] + poly[obj1].trans_tot[X]; center1[Y] = poly[obj1].mass_center[Y] + poly[obj1].trans_tot[Y]; center1[Z] = poly[obj1].mass_center[Z] + poly[obj1].trans_tot[Z]; center2[X] = poly[obj2].mass_center[X] + poly[obj2].trans_tot[X]; center2[Y] = poly[obj2].mass_center[Y] + poly[obj2].trans_tot[Y]; center2[Z] = poly[obj2].mass_center[Z] + poly[obj2].trans_tot[Z]; __col_vectSub(pt1, center1, vector1); __col_vectSub(pt2, center2, vector2); /* calculate the colliding points velocity and relative velocities */ /* __col_vectXprod(angle_velo1, vector1, velo1); __col_vectXprod(angle_velo2, vector2, velo2); */ __col_vectXprod(vector1, angle_velo1, velo1); __col_vectXprod(vector2, angle_velo2, velo2); __col_vectAdd(poly[obj1].trans_vel, velo1, velo1); __col_vectAdd(poly[obj2].trans_vel, velo2, velo2); __col_vectSub(velo1, velo2, rel_velo); /* make sure the colliding objects are not receding from each other */ if((Old_Delta_v[Z]=Delta_v[Z]=__col_vectDotProd(rel_velo, norm)) > 0) printf("Colliding objects are receding!\n"); else if(-Delta_v[Z] <= sqrt( 2.0*status->gravity*COL_COLLIDING_DISTANCE)) { printf("MICRO_COLLISION case handled here!!!\n"); /* printf("Before collision.\n"); printf("Obj1: [ (%lf, %lf, %lf) ]\n", poly[obj1].trans_vel[X], poly[obj1].trans_vel[Y], poly[obj1].trans_vel[Z]); printf("Obj2: [ (%lf, %lf, %lf) ]\n", poly[obj2].trans_vel[X], poly[obj1].trans_vel[Y], poly[obj1].trans_vel[Z]); */ /* get the collision velocity, reverse them, check impulse size */ Delta_v[X]=__col_vectDotProd(rel_velo, Xaxis); Delta_v[Y]=__col_vectDotProd(rel_velo, Yaxis); /* Delta_v[Z] = 0.0; */ for(i=0; i<3; i++) Delta_v[i] = -2.0*Delta_v[i]; frict = poly[obj1].friction + poly[obj2].friction; Mt[X][X] = Mat1[X][X] = Xaxis[X]; Mt[X][Y] = Mat1[X][Y] = Xaxis[Y]; Mt[X][Z] = Mat1[X][Z] = Xaxis[Z]; Mt[Y][X] = Mat1[Y][X] = Yaxis[X]; Mt[Y][Y] = Mat1[Y][Y] = Yaxis[Y]; Mt[Y][Z] = Mat1[Y][Z] = Yaxis[Z]; Mt[Z][X] = Mat1[Z][X] = norm[X]; Mt[Z][Y] = Mat1[Z][Y] = norm[Y]; Mt[Z][Z] = Mat1[Z][Z] = norm[Z]; mat3Invert(Mt, Mt_inv); /* transform the position vector for two colliding points */ __col_xform3(Mat1, vector1, VECTOR1); __col_xform3(Mat1, vector2, VECTOR2); get_col_matrix(Mat1, Mat2, obj1, obj2, VECTOR1, VECTOR2, Inert1, Inert2); mat3Add(Mat1, Mat2, Mat); mat3Invert(Mat, Mat_inv); /* obtain the impulse in the collision cordinates */ __col_xform3(Mat_inv, Delta_v, impulse_inv); /* obtain the impulse in the World cordinates */ __col_xform3(Mt_inv, impulse_inv, impulse); /* check if the frictional force is strong enough to reverse velocity */ if(__col_vectNorm(impulse) < frict) { printf("MICRO collision really happened\n"); tmp1 = 1.0/(poly[obj1].density*poly[obj1].volumn); tmp2 = 1.0/(poly[obj2].density*poly[obj2].volumn); for(i=0; i<3; i++) { imp1[i] = tmp1 * impulse[i]; imp2[i] = tmp2 * impulse[i]; } __col_vectAdd(poly[obj1].trans_vel, imp1, poly[obj1].trans_vel); __col_vectSub(poly[obj2].trans_vel, imp2, poly[obj2].trans_vel); __col_vectXprod(vector1, impulse, Angul1); __col_vectXprod(vector2, impulse, Angul2); __col_xform3(Inert1, Angul1, Delta_Ag1); __col_xform3(Inert2, Angul2, Delta_Ag2); __col_vectSub(angle_velo1, Delta_Ag1, angle_velo1); __col_vectAdd(angle_velo2, Delta_Ag2, angle_velo2); tmp1 = __col_vectNorm(angle_velo1); tmp2 = __col_vectNorm(angle_velo2); q_make(poly[obj1].delta_quat, angle_velo1[X], angle_velo1[Y], angle_velo1[Z], tmp1); q_make(poly[obj2].delta_quat, angle_velo2[X], angle_velo2[Y], angle_velo2[Z], tmp2); } /* printf("After collision.\n"); printf("Obj1: [ (%lf, %lf, %lf) ]\n", poly[obj1].trans_vel[X], poly[obj1].trans_vel[Y], poly[obj1].trans_vel[Z]); printf("Obj2: [ (%lf, %lf, %lf) ]\n", poly[obj2].trans_vel[X], poly[obj1].trans_vel[Y], poly[obj1].trans_vel[Z]); */ } else { /* Calculate the transformation matrix from world to colliding cordinates */ /* and pass the matrix to the following function get_col_matrix by Mat1. */ Mt[X][X] = Mat1[X][X] = Xaxis[X]; Mt[X][Y] = Mat1[X][Y] = Xaxis[Y]; Mt[X][Z] = Mat1[X][Z] = Xaxis[Z]; Mt[Y][X] = Mat1[Y][X] = Yaxis[X]; Mt[Y][Y] = Mat1[Y][Y] = Yaxis[Y]; Mt[Y][Z] = Mat1[Y][Z] = Yaxis[Z]; Mt[Z][X] = Mat1[Z][X] = norm[X]; Mt[Z][Y] = Mat1[Z][Y] = norm[Y]; Mt[Z][Z] = Mat1[Z][Z] = norm[Z]; mat3Invert(Mt, Mt_inv); /* transform the position vector for two colliding points */ __col_xform3(Mat1, vector1, VECTOR1); __col_xform3(Mat1, vector2, VECTOR2); get_col_matrix(Mat1, Mat2, obj1, obj2, VECTOR1, VECTOR2, Inert1, Inert2); mat3Add(Mat1, Mat2, Mat); mat3Invert(Mat, Mat_inv); Old_Delta_v[X]=Delta_v[X]=__col_vectDotProd(rel_velo, Xaxis); Old_Delta_v[Y]=Delta_v[Y]=__col_vectDotProd(rel_velo, Yaxis); frict = poly[obj1].friction + poly[obj2].friction; restit = poly[obj1].restit * poly[obj2].restit; /* start = clock(); */ cal_vel_change(Mat, Delta_v, frict, restit); /* finish = clock(); printf("one cycle costs: %f seconds.\n", ((double) (finish-start))/CLOCKS_PER_SEC); */ __col_vectSub(Delta_v, Old_Delta_v, Delta_v); /* obtain the impulse in the collision cordinates */ __col_xform3(Mat_inv, Delta_v, impulse_inv); /* obtain the impulse in the World cordinates */ __col_xform3(Mt_inv, impulse_inv, impulse); tmp1 = 1.0/(poly[obj1].density*poly[obj1].volumn); tmp2 = 1.0/(poly[obj2].density*poly[obj2].volumn); for(i=0; i<3; i++) { imp1[i] = tmp1 * impulse[i]; imp2[i] = tmp2 * impulse[i]; } __col_vectAdd(poly[obj1].trans_vel, imp1, poly[obj1].trans_vel); __col_vectSub(poly[obj2].trans_vel, imp2, poly[obj2].trans_vel); __col_vectXprod(vector1, impulse, Angul1); __col_vectXprod(vector2, impulse, Angul2); __col_xform3(Inert1, Angul1, Delta_Ag1); __col_xform3(Inert2, Angul2, Delta_Ag2); __col_vectSub(angle_velo1, Delta_Ag1, angle_velo1); __col_vectAdd(angle_velo2, Delta_Ag2, angle_velo2); tmp1 = __col_vectNorm(angle_velo1); tmp2 = __col_vectNorm(angle_velo2); q_make(poly[obj1].delta_quat, angle_velo1[X], angle_velo1[Y], angle_velo1[Z], tmp1); q_make(poly[obj2].delta_quat, angle_velo2[X], angle_velo2[Y], angle_velo2[Z], tmp2); } /* temp = poly[obj1].trans_vel[X]; poly[obj1].trans_vel[X] = poly[obj2].trans_vel[X]; poly[obj2].trans_vel[X] = temp; temp = poly[obj1].trans_vel[Y]; poly[obj1].trans_vel[Y] = poly[obj2].trans_vel[Y]; poly[obj2].trans_vel[Y] = temp; temp = poly[obj1].trans_vel[Z]; poly[obj1].trans_vel[Z] = poly[obj2].trans_vel[Z]; poly[obj2].trans_vel[Z] = temp; for (j=0; j<3; j++) quat_temp[j] = poly[obj1].delta_quat[j]; for (j=0; j<3; j++) poly[obj1].delta_quat[j] = poly[obj2].delta_quat[j]; for (j=0; j<3; j++) poly[obj2].delta_quat[j] = quat_temp[j]; */ } } #ifdef PRINT_COLLISIONS if (status->num_collisions) printf("\n"); #endif } /** End of compute_collision() **/ /*****************************************************************************\ @ move_polytopes() ----------------------------------------------------------------------------- description : Moves each polytope by amount described in its rotational and translational velocities. Checks to see if move exceeds bounding box. input : output : notes : \*****************************************************************************/ void move_polytopes(Status *status, Polytope *poly) { int i; double trans, tmp, tmp1, tmp2; double max_speed = 0.0; double center[3]; float min_dist = 1000.0; /* minimum distance between all pairs */ q_vec_type vect; q_type tmp_quat; double tick; int dist_pair; clock_t lapse, time; /* check the closet pair of objects. tick is the time lapse for each frame. Need to get the maximum possible transition velocity first to determine the next time tick in order to avoid overlapping of polytopes */ /* time = OLDTIME; OLDTIME = clock(); lapse = OLDTIME - time; printf("time is: %f seconds.\n", time); printf("oldtime is: %f seconds.\n", OLDTIME); printf("one cycle costs: %f seconds.\n", lapse/CLOCKS_PER_SEC); */ for (i= 0; i < status->num_polytopes; ++i) { vect_from_quat(poly[i].delta_quat, vect); tmp1 = __col_vectNorm(vect); tmp2 = __col_vectNorm(poly[i].trans_vel); if((tmp = (tmp2+tmp1*poly[i].max_radius+status->tick*status->gravity)) > max_speed) max_speed = tmp; } if((tmp = MIN_DIST/max_speed) > status->tick) tick = status->tick; else if(tmp > 0.001) tick = tmp; else tick = 0.001; /* accumulate the changes to the translation and rotation -- this has some error accumulation over a long period of time. */ for (i= 0; i < status->num_polytopes; ++i) { if(poly[i].move != 0) { /* update the angular velocity first */ vect_from_quat(poly[i].delta_quat, vect); /* tmp = status->correction * status->tick * __col_vectNorm(vect); */ tmp = status->correction * tick * __col_vectNorm(vect); q_make(tmp_quat, vect[X], vect[Y], vect[Z], tmp); q_mult(poly[i].tot_quat, poly[i].tot_quat, tmp_quat); /* q_mult(poly[i].tot_quat, poly[i].tot_quat, poly[i].delta_quat); */ /* poly[i].trans_tot[X] += status->correction * poly[i].trans_vel[X]*status->tick; poly[i].trans_tot[Y] += status->correction * poly[i].trans_vel[Y]*status->tick; */ poly[i].trans_tot[X] += status->correction * poly[i].trans_vel[X]*tick; poly[i].trans_tot[Y] += status->correction * poly[i].trans_vel[Y]*tick; /* Following code is for gravity influence in the Y direction */ /* poly[i].trans_tot[Y] -= status->correction*0.5*status->gravity*status->tick*status->tick; poly[i].trans_vel[Y] -= status->correction*status->gravity * status->tick; */ poly[i].trans_tot[Y] -= status->correction*0.5*status->gravity*tick*tick; poly[i].trans_vel[Y] -= status->correction*status->gravity * tick; /* poly[i].trans_tot[Z] += status->correction * poly[i].trans_vel[Z]*status->tick; */ poly[i].trans_tot[Z] += status->correction * poly[i].trans_vel[Z]*tick; center[X] = poly[i].mass_center[X] + poly[i].trans_tot[X]; center[Y] = poly[i].mass_center[Y] + poly[i].trans_tot[Y]; center[Z] = poly[i].mass_center[Z] + poly[i].trans_tot[Z]; /* Check for penetration of the walls of the simulation volume, and rebound off the walls if necessary. This is a bit of a hack, since it only keeps the object center within the walls. */ if (center[X] > cube_bound) { trans = center[X] - cube_bound; poly[i].trans_tot[X] -= trans; poly[i].trans_vel[X] = -poly[i].trans_vel[X]; } if (center[X] < -cube_bound) { trans = center[X] - (-cube_bound); poly[i].trans_tot[X] -= trans; poly[i].trans_vel[X] = -poly[i].trans_vel[X]; } if (center[Y] > cube_bound) { trans = center[Y] - cube_bound; poly[i].trans_tot[Y] -= trans; poly[i].trans_vel[Y] = -poly[i].trans_vel[Y]; } if (center[Y] < -cube_bound) { trans = center[Y] - (-cube_bound); poly[i].trans_tot[Y] -= trans; poly[i].trans_vel[Y] = -poly[i].trans_vel[Y]; } if (center[Z] > cube_bound) { trans = center[Z] - cube_bound; poly[i].trans_tot[Z] -= trans; poly[i].trans_vel[Z] = -poly[i].trans_vel[Z]; } if (center[Z] < -cube_bound) { trans = center[Z] - (-cube_bound); poly[i].trans_tot[Z] -= trans; poly[i].trans_vel[Z] = -poly[i].trans_vel[Z]; } } } } /** End of move_polytopes() **/ /*****************************************************************************\ @ main() ----------------------------------------------------------------------------- description : main program input : output : notes : \*****************************************************************************/ int main(int argc, char **argv) { float size = 1.00; int wire = 0; /* drawing flag. 1 for wire mode, 0 for fill */ int pause = 0; /* 1 for pausing the simulation. */ short val; col_Vect3 pt1, pt2, pt3; col_Vect3 pt4, pt5, pt6; col_Vect3 pt7, pt8; clock_t start, finish, duration; vinit("X11"); initialize(argc, argv, &status, &Poly); pt1[0] = -cube_bound; pt1[1] = cube_bound; pt1[2] = -cube_bound; pt2[0] = cube_bound; pt2[1] = cube_bound; pt2[2] = -cube_bound; pt3[0] = -cube_bound; pt3[1] = -cube_bound; pt3[2] = -cube_bound; pt4[0] = cube_bound; pt4[1] = -cube_bound; pt4[2] = -cube_bound; pt5[0] = cube_bound; pt5[1] = -cube_bound; pt5[2] = cube_bound; pt6[0] = cube_bound; pt6[1] = cube_bound; pt6[2] = cube_bound; pt7[0] = -cube_bound; pt7[1] = cube_bound; pt7[2] = cube_bound; pt8[0] = -cube_bound; pt8[1] = -cube_bound; pt8[2] = cube_bound; fflush(stdout); polymode(PYM_FILL); while (qread(&val) != REDRAW); compute_collision(&status, Poly); /* for (frames=0; frames < status.num_frames; frames++) */ while(1) { if(!pause) { color(BLACK); clear(); move_polytopes(&status, Poly); if(use_box) { draw_plane(pt2, pt1, pt4, 7, 4, 10.0, wire); draw_plane(pt1, pt2, pt7, 7, 4, 10.0, wire); draw_plane(pt2, pt4, pt6, 7, 4, 10.0, wire); draw_plane(pt8, pt5, pt3, 7, 4, 10.0, wire); draw_plane(pt3, pt1, pt8, 7, 4, 10.0, wire); } vogl_draw_scene(&status,size,wire); swapbuffers(); /* start = clock(); */ compute_collision(&status, Poly); /* finish = clock(); printf("one cycle costs: %f seconds.\n", ((double) (finish-start))/CLOCKS_PER_SEC); */ } if (qtest()) { (void)qread(&val); switch (val) { case 'q': /* Stop the program */ case 'Q': case 27: gexit(); exit(0); case 'w': /* Set wireframe mode */ wire = 1; break; case 'P': case 'p': /* pause the simulation */ pause = 1; break; case 'R': case 'r': /* resume the simulation */ pause = 0; break; case 's': wire = 0; /* Set solid mode */ break; case '>': /* zoom 5% */ size *= 1.05; break; case '<': /* zoom -5% */ size *= 0.95; break; default: ; } } } printf("done.\n"); printf("Total collisions: %d\n", total_collisions); printf("Average collisions per frame: %.3f\n", (double)total_collisions / (double)status.num_frames); return 0; } /** End of main() **/