// 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();
}
}
// not a full sort -- just makes column 1 the largest
int
eigen_and_sort1(double evecs[3][3], double cov[3][3])
{
double t;
double evals[3];
int n;
n = Meigen(evecs, evals, cov);
if (evals[2] > evals[0])
{
if (evals[2] > evals[1])
{
// 2 is largest, swap with column 0
t = evecs[0][2];
evecs[0][2] = evecs[0][0];
evecs[0][0] = t;
t = evecs[1][2];
evecs[1][2] = evecs[1][0];
evecs[1][0] = t;
t = evecs[2][2];
evecs[2][2] = evecs[2][0];
evecs[2][0] = t;
}
else
{
// 1 is largest, swap with column 0
t = evecs[0][1];
evecs[0][1] = evecs[0][0];
evecs[0][0] = t;
t = evecs[1][1];
evecs[1][1] = evecs[1][0];
evecs[1][0] = t;
t = evecs[2][1];
evecs[2][1] = evecs[2][0];
evecs[2][0] = t;
}
}
else
{
if (evals[0] > evals[1])
{
// 0 is largest, do nothing
}
else
{
// 1 is largest
t = evecs[0][1];
evecs[0][1] = evecs[0][0];
evecs[0][0] = t;
t = evecs[1][1];
evecs[1][1] = evecs[1][0];
evecs[1][0] = t;
t = evecs[2][1];
evecs[2][1] = evecs[2][0];
evecs[2][0] = t;
}
}
// we are returning the number of iterations Meigen took.
// too many iterations means our chosen orientation is bad.
return n;
}
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];
}
int
build_recurse(PQP_Model *m, int bn, int first_tri, int num_tris)
{
BV *b = m->child(bn);
// compute a rotation matrix
PQP_REAL C[3][3], E[3][3], R[3][3], s[3], axis[3], mean[3], coord;
#if RAPID2_FIT
moment *tri_moment = new moment[num_tris];
compute_moments(tri_moment, &(m->tris[first_tri]), num_tris);
accum acc;
clear_accum(acc);
for(int i = 0; i < num_tris; i++) accum_moment(acc, tri_moment[i]);
delete [] tri_moment;
covariance_from_accum(C,acc);
#else
get_covariance_triverts(C,&m->tris[first_tri],num_tris);
#endif
Meigen(E, s, C);
// place axes of E in order of increasing s
int min, mid, max;
if (s[0] > s[1]) { max = 0; min = 1; }
else { min = 0; max = 1; }
if (s[2] < s[min]) { mid = min; min = 2; }
else if (s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
McolcMcol(R,0,E,max);
McolcMcol(R,1,E,mid);
R[0][2] = E[1][max]*E[2][mid] - E[1][mid]*E[2][max];
R[1][2] = E[0][mid]*E[2][max] - E[0][max]*E[2][mid];
R[2][2] = E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
// fit the BV
b->FitToTris(R, &m->tris[first_tri], num_tris);
if (num_tris == 1)
{
// BV is a leaf BV - first_child will index a triangle
b->first_child = -(first_tri + 1);
}
else if (num_tris > 1)
{
// BV not a leaf - first_child will index a BV
b->first_child = m->num_bvs;
m->num_bvs+=2;
// choose splitting axis and splitting coord
McolcV(axis,R,0);
#if RAPID2_FIT
mean_from_accum(mean,acc);
#else
get_centroid_triverts(mean,&m->tris[first_tri],num_tris);
#endif
coord = VdotV(axis, mean);
// now split
int num_first_half = split_tris(&m->tris[first_tri], num_tris,
axis, coord);
// recursively build the children
build_recurse(m, m->child(bn)->first_child, first_tri, num_first_half);
build_recurse(m, m->child(bn)->first_child + 1,
first_tri + num_first_half, num_tris - num_first_half);
}
return PQP_OK;
}
