/* plane.c : This file contains the utility functions to get the colliding plane coordinates when two polytope touch each other, and the functions used to calculate the collision response */ /*----------------------------- Local Includes -----------------------------*/ #include #include #include #include #include #include "/cs1/shared/robotics/toolkit/I_COLLIDE/newpro/dist.c" #include "/cs1/shared/robotics/toolkit/I_COLLIDE/newpro/polytope.h" /*----------------------------- Local Constants -----------------------------*/ #if 0 #define PRINT_COLLISIONS #endif #define VERT_FLARE 0.5e-5 #define EDGE_V_FLARE 1.0e-5 #define EDGE_F_FLARE 1.0e-5 #define FACE_FLARE 2.0e-5 #define V 1 #define E 2 #define F 3 #define featTag(featurePtr) (*((int *) (featurePtr))) /*------------------------------ Local Macros -------------------------------*/ #define ABS(a) (((a) > 0.0) ? (a) : (-a)) /*------------------------------- Local Types -------------------------------*/ /*------------------------ Local Function Prototypes ------------------------*/ 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); double vect_from_quat(q_type quat, q_vec_type vect); void mat4To3Copy(col_Mat4 a, col_Mat3 b); void mat3Sub(col_Mat3 a, col_Mat3 b, col_Mat3 c); void mat3Add(col_Mat3 a, col_Mat3 b, col_Mat3 c); int mat3Invert(col_Mat3 mat, col_Mat3 invmat); 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); __col_Real vertex_vertex1(__col_Vertex **v1, __col_Vertex **v2, int obj1, int obj2, col_Mat4 T1, col_Mat4 T2, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2); __col_Real vertex_edge1(__col_Vertex **v, __col_Edge **e, int obj1, int obj2, col_Mat4 T12, col_Mat4 T21, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2); __col_Real edge_edge1(__col_Edge **e1, __col_Edge **e2, int obj1, int obj2, col_Mat4 T12, col_Mat4 T1, col_Mat4 T2, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2); __col_Real vertex_face1(__col_Vertex **v, __col_Face **f, int obj1, int obj2, col_Mat4 T12, col_Mat4 T1, col_Mat4 T2, __col_Polyhedron *facePoly, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2); __col_Real edge_face1(__col_Edge **e, __col_Face **f, int obj1, int obj2, col_Mat4 T12, col_Mat4 T1, col_Mat4 T2, __col_Polyhedron *facePoly, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2); __col_Real face_face1(__col_Face **f1, __col_Face **f2, int obj1, int obj2, col_Mat4 T12, col_Mat4 T21, col_Mat4 T1, col_Mat4 T2, __col_Polyhedron *face1poly, __col_Polyhedron *face2poly, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2); /*------------------------------ Local Globals ------------------------------*/ Status status; Polytope *Poly; extern __col_State state; extern __col_Polyhedron __col_polyhedronLibrary[1000]; /*---------------------------------Functions-------------------------------- */ /* function to calculate the collision plane and the colliding points colliding features are passed in. Two points and three coordinates are passed out */ 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) { int type1, type2; double dist, dist1; col_Mat4 T1, T2; col_Mat4 T12, T21; __col_matMultXform(state.polytopes[obj2].polytope->inv_pose, state.polytopes[obj1].polytope->pose, T12); __col_matMultXform(state.polytopes[obj1].polytope->inv_pose, state.polytopes[obj2].polytope->pose, T21); __col_mat4Copy(state.polytopes[obj1].polytope->pose, T1); __col_mat4Copy(state.polytopes[obj2].polytope->pose, T2); type1 = featTag(*feat1); type2 = featTag(*feat2); col_report_distance_pair(obj1, obj2, &(status->all_dist[0])); /* printf("Distance is: %16.14lf\n", status->all_dist[0].distance); printf("colliding ids: %d %d\n", obj1, obj2); printf("colliding feature 11:%d\n", type1); printf("colliding feature 22:%d\n", type2); __col_mat4Print(T1, "T1"); __col_mat4Print(T2, "T2"); */ dist1 = Distance(*feat1, *feat2, T12, T21); /* Defferent cases are handled here */ if (type1 == V) { if (type2 == V) { dist = vertex_vertex1((__col_Vertex **)feat1, (__col_Vertex **)feat2, obj1, obj2, T1, T2, norm, Xaxis, Yaxis, pt1, pt2); } else if (type2 == E) { dist = vertex_edge1((__col_Vertex **)feat1, (__col_Edge **)feat2, obj1, obj2, T1, T2, norm, Xaxis, Yaxis, pt1, pt2); } else { dist = vertex_face1((__col_Vertex **)feat1, (__col_Face **)feat2, obj1, obj2, T12, T1, T2, state.polytopes[obj2].polytope,norm,Xaxis,Yaxis,pt1, pt2); } } else if (type1 == E) { if (type2 == V) { dist = vertex_edge1((__col_Vertex **)feat2, (__col_Edge **)feat1, obj2, obj1, T2,T1,norm,Xaxis,Yaxis,pt2, pt1); __col_vectNeg(norm, norm); __col_vectNeg(Xaxis, Xaxis); } else if (type2 == E) { dist = edge_edge1((__col_Edge **)feat1, (__col_Edge **)feat2, obj1, obj2, T12, T1, T2,norm,Xaxis,Yaxis,pt1, pt2); } else { dist = edge_face1((__col_Edge **)feat1, (__col_Face **)feat2, obj1, obj2, T12, T1, T2, state.polytopes[obj2].polytope,norm,Xaxis,Yaxis,pt1, pt2); } } else { if (type2 == V) { dist = vertex_face1((__col_Vertex **)feat2, (__col_Face **)feat1, obj2, obj1, T21, T2, T1, state.polytopes[obj1].polytope, norm, Xaxis, Yaxis, pt2, pt1); __col_vectNeg(norm, norm); __col_vectNeg(Xaxis, Xaxis); } else if (type2 == E) { dist = edge_face1((__col_Edge **)feat2, (__col_Face **)feat1, obj2, obj1, T21, T2, T1, state.polytopes[obj1].polytope, norm, Xaxis, Yaxis, pt2, pt1); __col_vectNeg(norm, norm); __col_vectNeg(Xaxis, Xaxis); } else { dist = face_face1((__col_Face **)feat1, (__col_Face **)feat2, obj1, obj2, T12, T21, T1, T2, state.polytopes[obj1].polytope, state.polytopes[obj2].polytope, norm, Xaxis, Yaxis, pt1, pt2); } } return dist; } /*****************************************************************************\ @ vertex_vertex1() ----------------------------------------------------------------------------- description : calculate the collision plane in the case of vertex-vertex \*****************************************************************************/ __col_Real vertex_vertex1(__col_Vertex **v1, __col_Vertex **v2, int obj1, int obj2, col_Mat4 T1, col_Mat4 T2, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2) { col_Vect3 offset, center1, center2; col_Vect3 ruler; xformVert(T1, *v1); xformVert(T2, *v2); __col_vectSub((*v1)->xcoords, (*v2)->xcoords, offset); __col_vectNormalize(offset, norm); 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(center1, center2, ruler); if(__col_vectDotProd(norm, ruler) < 0.0) __col_vectNeg(norm, norm); xformEdge(T1, (*v1)->edges->f.e); __col_vectXprod(norm, (*v1)->edges->f.e->xu, Xaxis); __col_vectNormalize(Xaxis, Xaxis); __col_vectXprod(norm, Xaxis, Yaxis); __col_vectCopy((*v1)->xcoords, pt1); __col_vectCopy((*v2)->xcoords, pt2); return __col_vectNorm(offset); } /** End of vertex_vertex() **/ /*****************************************************************************\ @ vertex_edge1() ----------------------------------------------------------------------------- description : calculate the collision plane in the case of vertex_edge \*****************************************************************************/ __col_Real vertex_edge1(__col_Vertex **v, __col_Edge **e, int obj1, int obj2, col_Mat4 T1, col_Mat4 T2, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2) { col_Vect3 tmp, offset; __col_Real lambda; col_Vect3 ruler, center1, center2; xformVert(T1, *v); xformEdge(T2, *e); /* compute closest point on e to v */ __col_vectSub((*v)->xcoords, (*e)->v1->xcoords, tmp); lambda = __col_vectDotProd(tmp, (*e)->xu); if (lambda < 0.0) lambda = 0.0; else if (lambda > (*e)->len) lambda = (*e)->len; __col_displacePoint((*e)->v1->xcoords, (*e)->xu, lambda, tmp); __col_vectSub((*v)->xcoords, tmp, offset); __col_vectNormalize(offset, norm); 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(center1, center2, ruler); if(__col_vectDotProd(norm, ruler) < 0.0) __col_vectNeg(norm, norm); __col_vectXprod(norm, (*e)->xu, Xaxis); __col_vectNormalize(Xaxis, Xaxis); __col_vectXprod(norm, Xaxis, Yaxis); __col_vectCopy((*v)->xcoords, pt1); __col_vectCopy(tmp, pt2); return __col_vectNorm(offset); } /** End of vertex_edge() **/ /*****************************************************************************\ @ edge_edge1() ----------------------------------------------------------------------------- description : calculate the collision plane in the case of edge-edge \*****************************************************************************/ __col_Real edge_edge1(__col_Edge **e1, __col_Edge **e2, int obj1, int obj2, col_Mat4 T12, col_Mat4 T1, col_Mat4 T2, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2) { col_Vect3 cp1, cp2, offset, xformed1, xformed2; col_Vect3 ruler, center1, center2; xformEdge(T12, *e1); edgeCPs(e1, e2, cp1, cp2); __col_xformPoint(T2, cp1, xformed1); __col_xformPoint(T2, cp2, xformed2); __col_vectSub(xformed1, xformed2, offset); __col_vectNormalize(offset, norm); 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(center1, center2, ruler); if(__col_vectDotProd(norm, ruler) < 0.0) __col_vectNeg(norm, norm); xformEdge(T2, *e2); __col_vectXprod(norm, (*e2)->xu, Xaxis); __col_vectNormalize(Xaxis, Xaxis); __col_vectXprod(norm, Xaxis, Yaxis); __col_vectCopy(xformed1, pt1); __col_vectCopy(xformed2, pt2); return __col_vectNorm(offset); } /** End of edge_edge1() **/ /*****************************************************************************\ @ vertex_face1() ----------------------------------------------------------------------------- description : calculate the collision plane in the case of vertex-face \*****************************************************************************/ __col_Real vertex_face1(__col_Vertex **v, __col_Face **f, int obj1, int obj2, col_Mat4 T12, col_Mat4 T1, col_Mat4 T2, __col_Polyhedron *facePoly, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2) { col_Vect3 tmp, tmp2, offset; __col_Real dist; int flag = 0; col_Vect3 ruler, center1, center2; col_Vect3 key; key[X] = 1.0; key[Y] = 1.0; key[Z] = 1.0; xformVert(T12, *v); dist = __col_planeDist((*f)->plane, (*v)->xcoords); /* Compute closest point on f to v, i.e. proj. of v in f's plane. At this point, we know the projected point lies within f */ __col_vectScale((*f)->plane, tmp, dist); __col_vectSub((*v)->xcoords, tmp, tmp); xformVert(T1, *v); __col_xformPoint(T2, tmp, pt2); __col_vectCopy((*v)->xcoords, pt1); /* __col_vectSub(pt1, pt2, offset); */ __col_xformVect(T2, (*f)->plane, offset); __col_vectNormalize(offset, norm); /* 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(center1, center2, ruler); printf("inside vertex-face: %f\n\n\n", __col_vectDotProd(norm, ruler)); printf("center1: %f %f %f\n", center1[X], center1[Y], center1[Z]); printf("center2: %f %f %f\n", center2[X], center2[Y], center2[Z]); printf("norm: %f %f %f\n\n", norm[X], norm[Y], norm[Z]); if(__col_vectDotProd(norm, ruler) < 0.0) __col_vectNeg(norm, norm); */ xformEdge(T2, (*v)->edges->f.e); /* __col_vectXprod(norm, (*v)->edges->f.e->u, Xaxis); */ __col_vectXprod(norm, key, Xaxis); __col_vectNormalize(Xaxis, Xaxis); __col_vectXprod(norm, Xaxis, Yaxis); return dist; } /** End of vertex_face() **/ /*****************************************************************************\ @ edge_face1() ----------------------------------------------------------------------------- description : calculate the collision plane in the case of edge-face \*****************************************************************************/ __col_Real edge_face1(__col_Edge **e, __col_Face **f, int obj1, int obj2, col_Mat4 T12, col_Mat4 T1, col_Mat4 T2, __col_Polyhedron *facePoly, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2) { __col_Real h1, h2; col_Vect3 tmpV, tmpV1, tmpV2, offset; __col_Real min, max; __col_Real dist; col_Vect3 ruler, center1, center2; int i; /* compute distance of v1 and v2 from face */ xformEdge(T12, *e); h1 = __col_planeDist((*f)->plane, (*e)->v1->xcoords); h2 = __col_planeDist((*f)->plane, (*e)->v2->xcoords); /* fix for edges that would otherwise look || to f if h1 = -h2 */ if (((h1 < 0.0 && h2 > 0.0) || (h1 > 0.0 && h2 < 0.0)) && (fabs(h1 - h2) > 0.1001)) { dist = vertex_face1(&((*e)->v1), &(*f), obj1, obj2, T12, T1, T2, facePoly, norm, Xaxis, Yaxis, pt1, pt2); return dist; } /* case : e is parallel to face */ /* project v1 & v2 into plane of f */ if (!polygonCut(*f, *e, &min, &max)) { dist = vertex_face1(&((*e)->v1), &(*f), obj1, obj2, T12, T1, T2, facePoly, norm, Xaxis, Yaxis, pt1, pt2); return dist; } __col_displacePoint((*e)->v1->xcoords, (*e)->xu, min, tmpV1); __col_displacePoint((*e)->v1->xcoords, (*e)->xu, max, tmpV2); for(i=0; i<3; i++) tmpV[i] = (tmpV1[i] + tmpV2[i])/2.0; dist = __col_planeDist((*f)->plane, tmpV); __col_vectScale((*f)->plane, tmpV1, dist); __col_vectSub(tmpV, tmpV1, tmpV1); __col_xformPoint(T2, tmpV, pt1); __col_xformPoint(T2, tmpV1, pt2); /* __col_vectSub(pt1, pt2, offset); */ __col_xformVect(T2, (*f)->plane, offset); __col_vectNormalize(offset, norm); /* 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(center1, center2, ruler); if(__col_vectDotProd(norm, ruler) < 0.0) { __col_vectNeg(norm, norm); } */ xformEdge(T1, *e); __col_vectXprod(norm, (*e)->xu, Xaxis); __col_vectNormalize(Xaxis, Xaxis); __col_vectXprod(norm, Xaxis, Yaxis); return dist; } /** End of edge_face1() **/ /*****************************************************************************\ @ face_face1() ----------------------------------------------------------------------------- description : calculate the collision plane in the case of face-face \*****************************************************************************/ __col_Real face_face1(__col_Face **f1, __col_Face **f2, int obj1, int obj2, col_Mat4 T12, col_Mat4 T21, col_Mat4 T1, col_Mat4 T2, __col_Polyhedron *face1poly, __col_Polyhedron *face2poly, col_Vect3 norm, col_Vect3 Xaxis, col_Vect3 Yaxis, col_Vect3 pt1, col_Vect3 pt2) { __col_Edge *e1, *e; __col_Real min, max; __col_Real dist, minDist; col_Vect3 tmpV, tmpV2, ppt1, ppt2, offset; col_Fnode *fn; int flag = 0; col_Vect3 ruler, center1, center2; minDist = __COL_INFINITY; /* look at where f2's cone cuts f1's edges */ for (fn = (*f1)->edges; fn; fn = fn->next) { e = fn->f.e; xformEdge(T12, e); if (polygonCut(*f2, e, &min, &max)) { __col_displacePoint(e->v1->xcoords, e->xu, min, tmpV); dist = __col_planeDist((*f2)->plane, tmpV); if (dist < minDist) { if (dist < 0.0) printf("FACE-FACE ERROR, neg dist.\n"); __col_vectCopy(tmpV, ppt1); minDist = dist; e1 = e; __col_vectScale((*f2)->plane, tmpV2, dist); __col_vectSub(tmpV, tmpV2, ppt2); } __col_displacePoint(e->v1->xcoords, e->xu, max, tmpV); dist = __col_planeDist((*f2)->plane, tmpV); if (dist < minDist) { if (dist < 0.0) printf("FACE-FACE ERROR, neg dist.\n"); __col_vectCopy(tmpV, ppt1); minDist = dist; e1 = e; __col_vectScale((*f2)->plane, tmpV2, dist); __col_vectSub(tmpV, tmpV2, ppt2); } } } /* look at where f1's cone cuts f2's edges */ for (fn = (*f2)->edges; fn; fn = fn->next) { e = fn->f.e; xformEdge(T21, e); if (polygonCut(*f1, e, &min, &max)) { __col_displacePoint(e->v1->xcoords, e->xu, min, tmpV); dist = __col_planeDist((*f1)->plane, tmpV); if (dist < minDist) { flag = 1; if (dist < 0.0) printf("FACE-FACE ERROR, neg dist.\n"); __col_vectCopy(tmpV, ppt1); minDist = dist; e1 = e; __col_vectScale((*f2)->plane, tmpV2, dist); __col_vectSub(tmpV, tmpV2, ppt2); } __col_displacePoint(e->v1->xcoords, e->xu, max, tmpV); dist = __col_planeDist((*f1)->plane, tmpV); if (dist < minDist) { flag = 1; if (dist < 0.0) printf("FACE-FACE ERROR, neg dist.\n"); __col_vectCopy(tmpV, ppt1); minDist = dist; e1 = e; __col_vectScale((*f2)->plane, tmpV2, dist); __col_vectSub(tmpV, tmpV2, ppt2); } } } if(flag == 0) { __col_xformPoint(T2, ppt1, pt1); __col_xformPoint(T2, ppt2, pt2); xformEdge(T2, e); } else { __col_xformPoint(T1, ppt1, pt1); __col_xformPoint(T1, ppt2, pt2); xformEdge(T1, e); } /* __col_vectSub(pt1, pt2, offset); */ __col_xformVect(T2, (*f2)->plane, offset); __col_vectNormalize(offset, norm); /* 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(center1, center2, ruler); if(__col_vectDotProd(norm, ruler) < 0.0) __col_vectNeg(norm, norm); */ __col_vectXprod(norm, e->xu, Xaxis); __col_vectNormalize(Xaxis, Xaxis); __col_vectXprod(norm, Xaxis, Yaxis); } /** End of face_face1() **/ /* calculate the angular velocity form the quaternion */ double vect_from_quat(q_type quat, q_vec_type vect) { double angle; angle = 2.0 * acos(quat[W]); if(angle == 0.0) { vect[X] = 0.0; vect[Y] = 0.0; vect[Z] = 0.0; return; } vect[X] = angle*quat[X]/sin(angle/2.0); vect[Y] = angle*quat[Y]/sin(angle/2.0); vect[Z] = angle*quat[Z]/sin(angle/2.0); return angle; } /* function to calculate the matrix on the right side of the colliding collision equation: delta u = M x p(Yz) */ 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) { col_Mat3 mass1, mass2, skew1, skew2; col_Mat3 inmat1, inmat2, inmat11, inmat22; col_Mat3 inert_tmp1, inert_tmp2; col_Mat3 nert1, nert2, nert_tmp1, nert_tmp2; col_Mat3 inert1, inert2; double tmp1, tmp2, tmp; int i, j; skew1[X][X] = 0.0; skew2[X][X] = 0.0; skew1[X][Y] = -vector1[Z]; skew2[X][Y] = -vector2[Z]; skew1[X][Z] = vector1[Y]; skew2[X][Z] = vector2[Y]; skew1[Y][X] = vector1[Z]; skew2[Y][X] = vector2[Z]; skew1[Y][Y] = 0.0; skew2[Y][Y] = 0.0; skew1[Y][Z] = -vector1[X]; skew2[Y][Z] = -vector2[X]; skew1[Z][X] = -vector1[Y]; skew2[Z][X] = -vector2[Y]; skew1[Z][Y] = vector1[X]; skew2[Z][Y] = vector2[X]; skew1[Z][Z] = 0.0; skew2[Z][Z] = 0.0; tmp1 = 1.0/(Poly[obj1].density*Poly[obj1].volumn); tmp2 = 1.0/(Poly[obj2].density*Poly[obj2].volumn); __col_mat3Copy(__col_mat3IDENTITY, mass1); __col_mat3Copy(__col_mat3IDENTITY, mass2); for(i=0; i<3; i++) for(j=0; j<3; j++) { mass1[i][j] = tmp1 * mass1[i][j]; mass2[i][j] = tmp2 * mass2[i][j]; } /* transform to World coords first, then to the colliding coords */ mat4To3Copy(state.polytopes[obj1].polytope->pose, nert1); __col_mat3Mult(nert1, Poly[obj1].inertia, inert_tmp1); mat4To3Copy(state.polytopes[obj1].polytope->inv_pose, nert1); /* __col_mat3Mult(inert_tmp1, nert1, nert_tmp1); */ __col_mat3Mult(inert_tmp1, nert1, nert_tmp1); mat3Invert(nert_tmp1, Inert1); mat3Invert(mat1, mat2); __col_mat3Mult(mat1, nert_tmp1, nert1); __col_mat3Mult(nert1, mat2, nert_tmp1); mat3Invert(nert_tmp1, inert1); mat4To3Copy(state.polytopes[obj2].polytope->pose, nert2); __col_mat3Mult(nert2, Poly[obj2].inertia, inert_tmp2); mat4To3Copy(state.polytopes[obj2].polytope->inv_pose, nert2); __col_mat3Mult(inert_tmp2, nert2, nert_tmp2); mat3Invert(nert_tmp2, Inert2); __col_mat3Mult(mat1, nert_tmp2, nert2); __col_mat3Mult(nert2, mat2, nert_tmp2); mat3Invert(nert_tmp2, inert2); __col_mat3Mult(skew1, inert1, inmat11); __col_mat3Mult(inmat11, skew1, inmat1); __col_mat3Mult(skew2, inert2, inmat22); __col_mat3Mult(inmat22, skew2, inmat2); mat3Sub(mass1, inmat1, mat1); mat3Sub(mass2, inmat2, mat2); } /* matrix subtract a - b => c */ void mat3Sub(col_Mat3 a, col_Mat3 b, col_Mat3 c) { int i, j; for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) c[i][j] = a[i][j] - b[i][j]; } /** End of mat3Sub() **/ /* matrix addition a + b => c */ void mat3Add(col_Mat3 a, col_Mat3 b, col_Mat3 c) { int i, j; for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) c[i][j] = a[i][j] + b[i][j]; } /** End of mat3Add() **/ /* Invert an arbitrary 3x3 matrix */ int mat3Invert(col_Mat3 mat, col_Mat3 invmat) { double det; col_Mat3 cofac; cofac[0][0] = (mat[1][1]*mat[2][2] - mat[2][1]*mat[1][2]); cofac[0][1] = -(mat[1][0]*mat[2][2] - mat[2][0]*mat[1][2]); cofac[0][2] = (mat[1][0]*mat[2][1] - mat[2][0]*mat[1][1]); cofac[1][0] = -(mat[0][1]*mat[2][2] - mat[2][1]*mat[0][2]); cofac[1][1] = (mat[0][0]*mat[2][2] - mat[2][0]*mat[0][2]); cofac[1][2] = -(mat[0][0]*mat[2][1] - mat[2][0]*mat[0][1]); cofac[2][0] = (mat[0][1]*mat[1][2] - mat[1][1]*mat[0][2]); cofac[2][1] = -(mat[0][0]*mat[1][2] - mat[1][0]*mat[0][2]); cofac[2][2] = (mat[0][0]*mat[1][1] - mat[1][0]*mat[0][1]); det = mat[0][0]*cofac[0][0] + mat[0][1]*cofac[0][1] + mat[0][2]*cofac[0][2]; if (det == 0.) return -1; invmat[0][0] = cofac[0][0]/det; invmat[0][1] = cofac[1][0]/det; invmat[0][2] = cofac[2][0]/det; invmat[1][0] = cofac[0][1]/det; invmat[1][1] = cofac[1][1]/det; invmat[1][2] = cofac[2][1]/det; invmat[2][0] = cofac[0][2]/det; invmat[2][1] = cofac[1][2]/det; invmat[2][2] = cofac[2][2]/det; return 0; } /* Copy the rotation part of a transformation to another 3x3 matrix */ void mat4To3Copy(col_Mat4 a, col_Mat3 b) { int i, j; for(i=0; i<3; i++) for(j=0; j<3; j++) b[i][j] = a[i][j]; }