// 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, SWIFT_Triple& center,
bool compute_spreads,
SWIFT_Triple& min_dir, SWIFT_Real& min_spread,
SWIFT_Triple& mid_dir, SWIFT_Real& mid_spread,
SWIFT_Triple& max_dir, SWIFT_Real& max_spread )
{
int fn;
int* fs;
if( vs.Length() == 3 ) {
fs = new int[6];
fs[0] = 0; fs[1] = 1; fs[2] = 2;
fs[3] = 0; fs[4] = 2; fs[5] = 1;
fn = 2;
} else {
Compute_Convex_Hull( vs, fs, fn );
}
Compute_Spread( vs, fs, fn, false, center, min_dir, mid_dir, max_dir );
if( compute_spreads ) {
// Now we have compute the spreads. This can be done by inserting
// convex hull vertices into an OBB or by doing hill climbing on the
// convex hull. We will go for the first option since building the
// connectivity of the convex hull is too expensive.
int i;
SWIFT_Real min_min_spread = SWIFT_INFINITY;
SWIFT_Real min_mid_spread = SWIFT_INFINITY;
SWIFT_Real min_max_spread = SWIFT_INFINITY;
min_spread = -SWIFT_INFINITY;
mid_spread = -SWIFT_INFINITY;
max_spread = -SWIFT_INFINITY;
for( i = 0; i < fn*3; ) {
const SWIFT_Triple& v1 = vs[fs[i++]].Coords();
const SWIFT_Triple& v2 = vs[fs[i++]].Coords();
const SWIFT_Triple& v3 = vs[fs[i++]].Coords();
const SWIFT_Real min_dot1 = min_dir * v1;
const SWIFT_Real min_dot2 = min_dir * v2;
const SWIFT_Real min_dot3 = min_dir * v3;
const SWIFT_Real mid_dot1 = mid_dir * v1;
const SWIFT_Real mid_dot2 = mid_dir * v2;
const SWIFT_Real mid_dot3 = mid_dir * v3;
const SWIFT_Real max_dot1 = max_dir * v1;
const SWIFT_Real max_dot2 = max_dir * v2;
const SWIFT_Real max_dot3 = max_dir * v3;
Min_And_Max( min_min_spread, min_spread, min_dot1 );
Min_And_Max( min_min_spread, min_spread, min_dot2 );
Min_And_Max( min_min_spread, min_spread, min_dot3 );
Min_And_Max( min_mid_spread, mid_spread, mid_dot1 );
Min_And_Max( min_mid_spread, mid_spread, mid_dot2 );
Min_And_Max( min_mid_spread, mid_spread, mid_dot3 );
Min_And_Max( min_max_spread, max_spread, max_dot1 );
Min_And_Max( min_max_spread, max_spread, max_dot2 );
Min_And_Max( min_max_spread, max_spread, max_dot3 );
}
// Recompute the center
center = 0.5 * (SWIFT_Triple( max_spread, mid_spread, min_spread ) +
SWIFT_Triple( min_max_spread, min_mid_spread, min_min_spread ));
min_spread -= min_min_spread;
mid_spread -= min_mid_spread;
max_spread -= min_max_spread;
}
delete fs;
}
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];
}
