///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// /// /// FICHIER : moments.h /// /// /// /// NATURE : Manage the "moments" of a collision /// /// /// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// #ifndef MOMENTS_H #define MOMENTS_H #include "..\Collision\RAPID.H" #include "..\Collision\matvec.H" /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// /// moment STRUCTURE /// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// struct moment { float A; // area of the tri float m[3]; // centroid of the tri float s[3][3]; // covariance matrix of the tri }; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// /// accum STRUCTURE /// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// struct accum { float A; float m[3]; float s[3][3]; }; /////////////////////////////////////////////////////////////////////////// /// print_moment /// /////////////////////////////////////////////////////////////////////////// inline void print_moment(moment &M) { fprintf(stderr, "A: %g, m: %g %g %g, s: %g %g %g %g %g %g\n", M.A, M.m[0], M.m[1], M.m[2], M.s[0][0], M.s[0][1], M.s[0][2], M.s[1][1], M.s[1][2], M.s[2][2]); } /////////////////////////////////////////////////////////////////////////// /// clear_accum /// /////////////////////////////////////////////////////////////////////////// inline void clear_accum(accum &a) { a.m[0] = a.m[1] = a.m[2] = 0.0; a.s[0][0] = a.s[0][1] = a.s[0][2] = 0.0; a.s[1][0] = a.s[1][1] = a.s[1][2] = 0.0; a.s[2][0] = a.s[2][1] = a.s[2][2] = 0.0; a.A = 0.0; } /////////////////////////////////////////////////////////////////////////// /// accum_moment /// /////////////////////////////////////////////////////////////////////////// /// /// Accumule les moments d'un ensemble de triangles. /// Calcule l'aire TOTALE, le centre de gravité pondéré par les aires (non normalisé) /// et la matrice TOTALE (presque celle de covariance totale) /// inline void accum_moment(accum &a, moment &b) { a.m[0] += b.m[0] * b.A; a.m[1] += b.m[1] * b.A; a.m[2] += b.m[2] * b.A; a.s[0][0] += b.s[0][0]; a.s[0][1] += b.s[0][1]; a.s[0][2] += b.s[0][2]; a.s[1][0] += b.s[1][0]; a.s[1][1] += b.s[1][1]; a.s[1][2] += b.s[1][2]; a.s[2][0] += b.s[2][0]; a.s[2][1] += b.s[2][1]; a.s[2][2] += b.s[2][2]; a.A += b.A; } /////////////////////////////////////////////////////////////////////////// /// mean_from_moment /// /////////////////////////////////////////////////////////////////////////// /// /// Renvoit la position du centre de gravité d'UN seul triangle /// inline void mean_from_moment(float M[3], moment &m) { M[0] = m.m[0]; M[1] = m.m[1]; M[2] = m.m[2]; } /////////////////////////////////////////////////////////////////////////// /// mean_from_accum /// /////////////////////////////////////////////////////////////////////////// /// /// Renvoit la position du centre de gravité de l'ensemble des triangles /// (barycentre pondéré par les aires des triangles) /// inline void mean_from_accum(float M[3], accum &a) { M[0] = a.m[0] / a.A; M[1] = a.m[1] / a.A; M[2] = a.m[2] / a.A; } /////////////////////////////////////////////////////////////////////////// /// covariance_from_accum /// /////////////////////////////////////////////////////////////////////////// /// /// Calcule la matrice de covariance pour l'ensemble des triangles /// inline void covariance_from_accum(float C[3][3], accum &a) { int i,j; for(i=0; i<3; i++) for(j=0; j<3; j++) C[i][j] = a.s[i][j] - a.m[i]*a.m[j]/a.A; } /////////////////////////////////////////////////////////////////////////// /// compute_moment /// /////////////////////////////////////////////////////////////////////////// /// /// Calcule l'aire, le centre de gravité et la matrice de covariance (presque) /// pour UN seul triangle /// inline void compute_moment(moment &M, float p[3], float q[3], float r[3]) { float u[3], v[3], w[3]; // compute the area of the triangle VmV(u, q, p); VmV(v, r, p); VcrossV(w, u, v); M.A = 0.5f * (float)Vlength(w); if (M.A == 0.0) { // This triangle has zero area. The second order components // would be eliminated with the usual formula, so, for the // sake of robustness we use an alternative form. These are the // centroid and second-order components of the triangle's vertices. // centroid M.m[0] = (p[0] + q[0] + r[0]) /3; M.m[1] = (p[1] + q[1] + r[1]) /3; M.m[2] = (p[2] + q[2] + r[2]) /3; // second-order components M.s[0][0] = (p[0]*p[0] + q[0]*q[0] + r[0]*r[0]); M.s[0][1] = (p[0]*p[1] + q[0]*q[1] + r[0]*r[1]); M.s[0][2] = (p[0]*p[2] + q[0]*q[2] + r[0]*r[2]); M.s[1][1] = (p[1]*p[1] + q[1]*q[1] + r[1]*r[1]); M.s[1][2] = (p[1]*p[2] + q[1]*q[2] + r[1]*r[2]); M.s[2][2] = (p[2]*p[2] + q[2]*q[2] + r[2]*r[2]); M.s[2][1] = M.s[1][2]; M.s[1][0] = M.s[0][1]; M.s[2][0] = M.s[0][2]; return; } // get the centroid M.m[0] = (p[0] + q[0] + r[0])/3; M.m[1] = (p[1] + q[1] + r[1])/3; M.m[2] = (p[2] + q[2] + r[2])/3; // get the second order components -- note the weighting by the area M.s[0][0] = M.A*(9*M.m[0]*M.m[0]+p[0]*p[0]+q[0]*q[0]+r[0]*r[0])/12; M.s[0][1] = M.A*(9*M.m[0]*M.m[1]+p[0]*p[1]+q[0]*q[1]+r[0]*r[1])/12; M.s[1][1] = M.A*(9*M.m[1]*M.m[1]+p[1]*p[1]+q[1]*q[1]+r[1]*r[1])/12; M.s[0][2] = M.A*(9*M.m[0]*M.m[2]+p[0]*p[2]+q[0]*q[2]+r[0]*r[2])/12; M.s[1][2] = M.A*(9*M.m[1]*M.m[2]+p[1]*p[2]+q[1]*q[2]+r[1]*r[2])/12; M.s[2][2] = M.A*(9*M.m[2]*M.m[2]+p[2]*p[2]+q[2]*q[2]+r[2]*r[2])/12; M.s[2][1] = M.s[1][2]; M.s[1][0] = M.s[0][1]; M.s[2][0] = M.s[0][2]; } /////////////////////////////////////////////////////////////////////////// /// compute_moments /// /////////////////////////////////////////////////////////////////////////// /// /// Idem que précédemment, mais pour un ensemble de triangles /// inline void compute_moments(moment *M, tri *tris, int num_tris) { int i; // first collect all the moments, and obtain the area of the // smallest nonzero area triangle. float Amin = 0.0; int zero = 0; int nonzero = 0; for(i=0; i