void Compute_Piece_Centers_Of_Mass( )
{
int i, j;
SWIFT_Real area;
SWIFT_Real total_area;
SWIFT_Triple areav;
SWIFT_Triple com;
if( model_faces.Length() != 0 ) {
piece_coms.Create( model_faces.Length() );
for( i = 0; i < model_faces.Length(); i++ ) {
com.Set_Value( 0.0, 0.0, 0.0 );
total_area = 0.0;
for( j = 0; j < model_faces[i].Length(); j++ ) {
areav = (mesh->Faces()[model_faces[i][j]].Edge1().Origin()->Coords() -
mesh->Faces()[model_faces[i][j]].Edge2().Origin()->Coords()) %
(mesh->Faces()[model_faces[i][j]].Edge1().Origin()->Coords() -
mesh->Faces()[model_faces[i][j]].Edge3().Origin()->Coords());
area = 0.5 * areav.Length();
total_area += area;
com += area * (mesh->Faces()[model_faces[i][j]].Edge1().Origin()->Coords() +
mesh->Faces()[model_faces[i][j]].Edge2().Origin()->Coords() +
mesh->Faces()[model_faces[i][j]].Edge3().Origin()->Coords() );
}
for( j = 0; j < virtual_faces[i].Length(); j++ ) {
areav = (virtual_faces[i][j].Edge1().Origin()->Coords() -
virtual_faces[i][j].Edge2().Origin()->Coords()) %
(virtual_faces[i][j].Edge1().Origin()->Coords() -
virtual_faces[i][j].Edge3().Origin()->Coords());
area = 0.5 * areav.Length();
total_area += area;
com += area * (virtual_faces[i][j].Edge1().Origin()->Coords() +
virtual_faces[i][j].Edge2().Origin()->Coords() +
virtual_faces[i][j].Edge3().Origin()->Coords() );
}
piece_coms[i] = com / (3.0 * total_area);
}
}
}
// This routine computes the spread of a closed convex polyhedron.
// The vertices, faces, and center of mass (centroids weighted by area)
// are given. Resulting from eigen-vectors are returned.
// This algorithm was derived from the SIGGRAPH 96 paper on OBB's.
void Compute_Spread( SWIFT_Array<SWIFT_Tri_Vertex>& vs, int* fs, int fn,
bool have_com,
SWIFT_Triple& com, SWIFT_Triple& min_dir,
SWIFT_Triple& mid_dir, SWIFT_Triple& max_dir )
{
int i;
int mine, mide, maxe;
SWIFT_Real area_x2;
SWIFT_Real total_area;
SWIFT_Real s[3];
SWIFT_Real C[3][3];
SWIFT_Real E[3][3];
SWIFT_Triple centroid;
SWIFT_Triple areav;
// Create the covariance matrix
C[0][0] = C[0][1] = C[0][2] = C[1][1] = C[1][2] = C[2][2] = 0.0;
total_area = 0.0;
if( have_com ) {
for( i = 0; i < fn*3; ) {
const SWIFT_Triple& vx = vs[fs[i++]].Coords();
const SWIFT_Triple& vy = vs[fs[i++]].Coords();
const SWIFT_Triple& vz = vs[fs[i++]].Coords();
areav = (vx - vy) % (vx - vz);
area_x2 = areav.Length();
total_area += area_x2;
centroid = vx + vy + vz;
C[0][0] += area_x2 * (centroid.X() * centroid.X() +
vx.X() * vx.X() + vy.X() * vy.X() + vz.X() * vz.X());
C[0][1] += area_x2 * (centroid.X() * centroid.Y() +
vx.X() * vx.Y() + vy.X() * vy.Y() + vz.X() * vz.Y());
C[0][2] += area_x2 * (centroid.X() * centroid.Z() +
vx.X() * vx.Z() + vy.X() * vy.Z() + vz.X() * vz.Z());
C[1][1] += area_x2 * (centroid.Y() * centroid.Y() +
vx.Y() * vx.Y() + vy.Y() * vy.Y() + vz.Y() * vz.Y());
C[1][2] += area_x2 * (centroid.Y() * centroid.Z() +
vx.Y() * vx.Z() + vy.Y() * vy.Z() + vz.Y() * vz.Z());
C[2][2] += area_x2 * (centroid.Z() * centroid.Z() +
vx.Z() * vx.Z() + vy.Z() * vy.Z() + vz.Z() * vz.Z());
}
} else {
com.Set_Value( 0.0, 0.0, 0.0 );
for( i = 0; i < fn*3; ) {
const SWIFT_Triple& vx = vs[fs[i++]].Coords();
const SWIFT_Triple& vy = vs[fs[i++]].Coords();
const SWIFT_Triple& vz = vs[fs[i++]].Coords();
areav = (vx - vy) % (vx - vz);
area_x2 = areav.Length();
total_area += area_x2;
centroid = vx + vy + vz;
com += area_x2 * (vx + vy + vz);
C[0][0] += area_x2 * (centroid.X() * centroid.X() +
vx.X() * vx.X() + vy.X() * vy.X() + vz.X() * vz.X());
C[0][1] += area_x2 * (centroid.X() * centroid.Y() +
vx.X() * vx.Y() + vy.X() * vy.Y() + vz.X() * vz.Y());
C[0][2] += area_x2 * (centroid.X() * centroid.Z() +
vx.X() * vx.Z() + vy.X() * vy.Z() + vz.X() * vz.Z());
C[1][1] += area_x2 * (centroid.Y() * centroid.Y() +
vx.Y() * vx.Y() + vy.Y() * vy.Y() + vz.Y() * vz.Y());
C[1][2] += area_x2 * (centroid.Y() * centroid.Z() +
vx.Y() * vx.Z() + vy.Y() * vy.Z() + vz.Y() * vz.Z());
C[2][2] += area_x2 * (centroid.Z() * centroid.Z() +
vx.Z() * vx.Z() + vy.Z() * vy.Z() + vz.Z() * vz.Z());
}
com /= 3.0 * total_area;
}
total_area *= 0.5;
C[0][0] = C[0][0] / 24.0 - com.X() * com.X() * total_area;
C[0][1] = C[0][1] / 24.0 - com.X() * com.Y() * total_area;
C[1][0] = C[0][1];
C[0][2] = C[0][2] / 24.0 - com.X() * com.Z() * total_area;
C[2][0] = C[0][2];
C[1][1] = C[1][1] / 24.0 - com.Y() * com.Y() * total_area;
C[1][2] = C[1][2] / 24.0 - com.Y() * com.Z() * total_area;
C[2][1] = C[1][2];
C[2][2] = C[2][2] / 24.0 - com.Z() * com.Z() * total_area;
// Do eigen-analysis to find the major/minor axes of the object
Meigen( E, s, C );
// Compare the eigen values and sort them
if (s[0] > s[1]) { maxe = 0; mine = 1; }
else { mine = 0; maxe = 1; }
if (s[2] < s[mine]) { mide = mine; mine = 2; }
else if (s[2] > s[maxe]) { mide = maxe; maxe = 2; }
else { mide = 2; }
min_dir = SWIFT_Triple( E[0][mine], E[1][mine], E[2][mine] );
mid_dir = SWIFT_Triple( E[0][mide], E[1][mide], E[2][mide] );
max_dir = SWIFT_Triple( E[0][maxe], E[1][maxe], E[2][maxe] );
// Ensure a rotation matrix
if( (max_dir % mid_dir) * min_dir < 0.0 ) {
max_dir.Negate();
}
}
void Compute_Spread( SWIFT_Array<SWIFT_Tri_Vertex>& vs, int* fs, int fn,
const SWIFT_Triple& com,
SWIFT_Triple& min_dir, SWIFT_Real& min_spread,
SWIFT_Triple& max_dir, SWIFT_Real& max_spread )
{
int i;
int mine, mide, maxe;
SWIFT_Real area;
SWIFT_Real total_area;
SWIFT_Real s[3];
SWIFT_Real C[3][3];
SWIFT_Real E[3][3];
SWIFT_Triple centroid;
SWIFT_Triple areav;
// Create the covariance matrix
C[0][0] = 0.0; C[0][1] = 0.0; C[0][2] = 0.0;
C[1][0] = 0.0; C[1][1] = 0.0; C[1][2] = 0.0;
C[2][0] = 0.0; C[2][1] = 0.0; C[2][2] = 0.0;
total_area = 0.0;
for( i = 0; i < fn*3; ) {
const SWIFT_Triple& vx = vs[fs[i++]].Coords();
const SWIFT_Triple& vy = vs[fs[i++]].Coords();
const SWIFT_Triple& vz = vs[fs[i++]].Coords();
areav = (vx - vy) % (vx - vz);
area = 0.5 * areav.Length();
total_area += area;
centroid = vx + vy + vz;
C[0][0] += area * (centroid.X() * centroid.X()+
vx.X() * vx.X()+ vy.X() * vy.X()+ vz.X() * vz.X());
C[0][1] += area * (centroid.X() * centroid.Y()+
vx.X() * vx.Y()+ vy.X() * vy.Y()+ vz.X() * vz.Y());
C[0][2] += area * (centroid.X() * centroid.Z()+
vx.X() * vx.Z()+ vy.X() * vy.Z()+ vz.X() * vz.Z());
C[1][1] += area * (centroid.Y() * centroid.Y()+
vx.Y() * vx.Y()+ vy.Y() * vy.Y()+ vz.Y() * vz.Y());
C[1][2] += area * (centroid.Y() * centroid.Z()+
vx.Y() * vx.Z()+ vy.Y() * vy.Z()+ vz.Y() * vz.Z());
C[2][2] += area * (centroid.Z() * centroid.Z()+
vx.Z() * vx.Z()+ vy.Z() * vy.Z()+ vz.Z() * vz.Z());
}
C[0][0] = C[0][0] / 12.0 - com.X() * com.X() * total_area;
C[0][1] = C[0][1] / 12.0 - com.X() * com.Y() * total_area;
C[0][2] = C[0][2] / 12.0 - com.X() * com.Z() * total_area;
C[1][1] = C[1][1] / 12.0 - com.Y() * com.Y() * total_area;
C[1][2] = C[1][2] / 12.0 - com.Y() * com.Z() * total_area;
C[2][2] = C[2][2] / 12.0 - com.Z() * com.Z() * total_area;
C[1][0] = C[0][1];
C[2][0] = C[0][2];
C[2][1] = C[1][2];
// Do eigen-analysis to find the major/minor axes of the object
Meigen( E, s, C );
// Compare the eigen values and sort them
if (s[0] > s[1]) { maxe = 0; mine = 1; }
else { mine = 0; maxe = 1; }
if (s[2] < s[mine]) { mide = mine; mine = 2; }
else if (s[2] > s[maxe]) { mide = maxe; maxe = 2; }
else { mide = 2; }
min_dir = SWIFT_Triple( E[0][mine], E[1][mine], E[2][mine] );
min_spread = s[mine];
max_dir = SWIFT_Triple( E[0][maxe], E[1][maxe], E[2][maxe] );
max_spread = s[maxe];
}