int
RAPID_model::build_hierarchy()
{
// allocate the boxes and set the box list globals
num_boxes_alloced = num_tris * 2;
b = new box[num_boxes_alloced];
if (b == 0) return RAPID_ERR_MODEL_OUT_OF_MEMORY;
RAPID_boxes = b;
RAPID_boxes_inited = 1; // we are in process of initializing b[0].
// Determine initial orientation, mean point, and splitting axis.
int i;
accum M;
// double F1[3];
// double S1[6];
double C[3][3];
RAPID_moment = new moment[num_tris];
if (RAPID_moment == 0)
{
delete [] b;
return RAPID_ERR_MODEL_OUT_OF_MEMORY;
}
compute_moments(RAPID_moment, tris, num_tris);
clear_accum(M);
for(i=0; i<num_tris; i++)
accum_moment(M, RAPID_moment[i]);
mean_from_accum(b[0].pT, M);
covariance_from_accum(C, M);
eigen_and_sort1(b[0].pR, C);
// create the index list
int *t = new int[num_tris];
if (t == 0)
{
delete [] b;
delete [] RAPID_moment;
return RAPID_ERR_MODEL_OUT_OF_MEMORY;
}
for(i=0; i<num_tris; i++) t[i] = i;
// set the tri pointer
RAPID_tri = tris;
// do the build
int rc = b[0].split_recurse(t, num_tris);
if (rc != RAPID_OK)
{
delete [] b;
delete [] RAPID_moment;
delete [] t;
return RAPID_ERR_MODEL_OUT_OF_MEMORY;
}
// free the moment list
delete [] RAPID_moment; RAPID_moment = 0;
// null the tri pointer
RAPID_tri = 0;
// free the index list
delete [] t;
return RAPID_OK;
}
int
box::split_recurse(int *t, int n)
{
// The orientation for the parent box is already assigned to this->pR.
// The axis along which to split will be column 0 of this->pR.
// The mean point is passed in on this->pT.
// When this routine completes, the position and orientation in model
// space will be established, as well as its dimensions. Child boxes
// will be constructed and placed in the parent's CS.
if (n == 1)
{
return split_recurse(t);
}
// walk along the tris for the box, and do the following:
// 1. collect the max and min of the vertices along the axes of <or>.
// 2. decide which group the triangle goes in, performing appropriate swap.
// 3. accumulate the mean point and covariance data for that triangle.
accum M1, M2;
double C[3][3];
double c[3];
double minval[3], maxval[3];
int rc; // for return code on procedure calls.
int in;
tri *ptr;
int i;
double axdmp;
int n1 = 0; // The number of tris in group 1.
// Group 2 will have n - n1 tris.
// project approximate mean point onto splitting axis, and get coord.
axdmp = (pR[0][0] * pT[0] + pR[1][0] * pT[1] + pR[2][0] * pT[2]);
clear_accum(M1);
clear_accum(M2);
MTxV(c, pR, RAPID_tri[t[0]].p1);
minval[0] = maxval[0] = c[0];
minval[1] = maxval[1] = c[1];
minval[2] = maxval[2] = c[2];
for(i=0; i<n; i++)
{
in = t[i];
ptr = RAPID_tri + in;
MTxV(c, pR, ptr->p1);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p2);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p3);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
// grab the mean point of the in'th triangle, project
// it onto the splitting axis (1st column of pR) and
// see where it lies with respect to axdmp.
mean_from_moment(c, RAPID_moment[in]);
if (((pR[0][0]*c[0] + pR[1][0]*c[1] + pR[2][0]*c[2]) < axdmp)
&& ((n!=2)) || ((n==2) && (i==0)))
{
// accumulate first and second order moments for group 1
accum_moment(M1, RAPID_moment[in]);
// put it in group 1 by swapping t[i] with t[n1]
int temp = t[i];
t[i] = t[n1];
t[n1] = temp;
n1++;
}
else
{
// accumulate first and second order moments for group 2
accum_moment(M2, RAPID_moment[in]);
// leave it in group 2
// do nothing...it happens by default
}
}
// done using this->pT as a mean point.
// error check!
if ((n1 == 0) || (n1 == n))
{
// our partitioning has failed: all the triangles fell into just
// one of the groups. So, we arbitrarily partition them into
// equal parts, and proceed.
n1 = n/2;
// now recompute accumulated stuff
reaccum_moments(M1, t, n1);
reaccum_moments(M2, t + n1, n - n1);
}
// With the max and min data, determine the center point and dimensions
// of the parent box.
c[0] = (minval[0] + maxval[0])*0.5;
c[1] = (minval[1] + maxval[1])*0.5;
c[2] = (minval[2] + maxval[2])*0.5;
pT[0] = c[0] * pR[0][0] + c[1] * pR[0][1] + c[2] * pR[0][2];
pT[1] = c[0] * pR[1][0] + c[1] * pR[1][1] + c[2] * pR[1][2];
pT[2] = c[0] * pR[2][0] + c[1] * pR[2][1] + c[2] * pR[2][2];
d[0] = (maxval[0] - minval[0])*0.5;
d[1] = (maxval[1] - minval[1])*0.5;
d[2] = (maxval[2] - minval[2])*0.5;
// allocate new boxes
P = RAPID_boxes + RAPID_boxes_inited++;
N = RAPID_boxes + RAPID_boxes_inited++;
// Compute the orienations for the child boxes (eigenvectors of
// covariance matrix). Select the direction of maximum spread to be
// the split axis for each child.
double tR[3][3];
if (n1 > 1)
{
mean_from_accum(P->pT, M1);
covariance_from_accum(C, M1);
if (eigen_and_sort1(tR, C) > 30)
{
// unable to find an orientation. We'll just pick identity.
Midentity(tR);
}
McM(P->pR, tR);
if ((rc = P->split_recurse(t, n1)) != RAPID_OK) return rc;
}
else
{
if ((rc = P->split_recurse(t)) != RAPID_OK) return rc;
}
McM(C, P->pR); MTxM(P->pR, pR, C); // and F1
VmV(c, P->pT, pT); MTxV(P->pT, pR, c);
if ((n-n1) > 1)
{
mean_from_accum(N->pT, M2);
covariance_from_accum (C, M2);
if (eigen_and_sort1(tR, C) > 30)
{
// unable to find an orientation. We'll just pick identity.
Midentity(tR);
}
McM(N->pR, tR);
if ((rc = N->split_recurse(t + n1, n - n1)) != RAPID_OK) return rc;
}
else
{
if ((rc = N->split_recurse(t+n1)) != RAPID_OK) return rc;
}
McM(C, N->pR); MTxM(N->pR, pR, C);
VmV(c, N->pT, pT); MTxV(N->pT, pR, c);
return RAPID_OK;
}
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;
}