int
split_tris(Tri *tris, int num_tris, PQP_REAL a[3], PQP_REAL c)
{
int i;
int c1 = 0;
PQP_REAL p[3];
PQP_REAL x;
Tri temp;
for(i = 0; i < num_tris; i++)
{
// loop invariant: up to (but not including) index c1 in group 1,
// then up to (but not including) index i in group 2
//
// [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
// c1 i
//
VcV(p, tris[i].p1);
VpV(p, p, tris[i].p2);
VpV(p, p, tris[i].p3);
x = VdotV(p, a);
x /= 3.0;
if (x <= c)
{
// group 1
temp = tris[i];
tris[i] = tris[c1];
tris[c1] = temp;
c1++;
}
else
{
// group 2 -- do nothing
}
}
// split arbitrarily if one group empty
if ((c1 == 0) || (c1 == num_tris)) c1 = num_tris/2;
return c1;
}
int
project6(PQP_REAL *ax,
PQP_REAL *p1, PQP_REAL *p2, PQP_REAL *p3,
PQP_REAL *q1, PQP_REAL *q2, PQP_REAL *q3)
{
PQP_REAL P1 = VdotV(ax, p1);
PQP_REAL P2 = VdotV(ax, p2);
PQP_REAL P3 = VdotV(ax, p3);
PQP_REAL Q1 = VdotV(ax, q1);
PQP_REAL Q2 = VdotV(ax, q2);
PQP_REAL Q3 = VdotV(ax, q3);
PQP_REAL mx1 = max(P1, P2, P3);
PQP_REAL mn1 = min(P1, P2, P3);
PQP_REAL mx2 = max(Q1, Q2, Q3);
PQP_REAL mn2 = min(Q1, Q2, Q3);
if (mn1 > mx2) return 0;
if (mn2 > mx1) return 0;
return 1;
}
int
project6(float *ax,
float *p1, float *p2, float *p3,
float *q1, float *q2, float *q3)
{
float P1 = VdotV(ax, p1);
float P2 = VdotV(ax, p2);
float P3 = VdotV(ax, p3);
float Q1 = VdotV(ax, q1);
float Q2 = VdotV(ax, q2);
float Q3 = VdotV(ax, q3);
float mx1 = maxRapid(P1, P2, P3);
float mn1 = min(P1, P2, P3);
float mx2 = maxRapid(Q1, Q2, Q3);
float mn2 = min(Q1, Q2, Q3);
if (mn1 > mx2) return 0;
if (mn2 > mx1) return 0;
return 1;
}
void
SegPoints(PQP_REAL VEC[3],
PQP_REAL X[3], PQP_REAL Y[3], // closest points
const PQP_REAL P[3], const PQP_REAL A[3], // seg 1 origin, vector
const PQP_REAL Q[3], const PQP_REAL B[3]) // seg 2 origin, vector
{
PQP_REAL T[3], A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
PQP_REAL TMP[3];
VmV(T,Q,P);
A_dot_A = VdotV(A,A);
B_dot_B = VdotV(B,B);
A_dot_B = VdotV(A,B);
A_dot_T = VdotV(A,T);
B_dot_T = VdotV(B,T);
// t parameterizes ray P,A
// u parameterizes ray Q,B
PQP_REAL t,u;
// compute t for the closest point on ray P,A to
// ray Q,B
PQP_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
// clamp result so t is on the segment P,A
if ((t < 0) || isnan(t)) t = 0; else if (t > 1) t = 1;
// find u for point on ray Q,B closest to point at t
u = (t*A_dot_B - B_dot_T) / B_dot_B;
// if u is on segment Q,B, t and u correspond to
// closest points, otherwise, clamp u, recompute and
// clamp t
if ((u <= 0) || isnan(u)) {
VcV(Y, Q);
t = A_dot_T / A_dot_A;
if ((t <= 0) || isnan(t)) {
VcV(X, P);
VmV(VEC, Q, P);
}
else if (t >= 1) {
VpV(X, P, A);
VmV(VEC, Q, X);
}
else {
VpVxS(X, P, A, t);
VcrossV(TMP, T, A);
VcrossV(VEC, A, TMP);
}
}
else if (u >= 1) {
VpV(Y, Q, B);
t = (A_dot_B + A_dot_T) / A_dot_A;
if ((t <= 0) || isnan(t)) {
VcV(X, P);
VmV(VEC, Y, P);
}
else if (t >= 1) {
VpV(X, P, A);
VmV(VEC, Y, X);
}
else {
VpVxS(X, P, A, t);
VmV(T, Y, P);
VcrossV(TMP, T, A);
VcrossV(VEC, A, TMP);
}
}
else {
VpVxS(Y, Q, B, u);
if ((t <= 0) || isnan(t)) {
VcV(X, P);
VcrossV(TMP, T, B);
VcrossV(VEC, B, TMP);
}
else if (t >= 1) {
VpV(X, P, A);
VmV(T, Q, X);
VcrossV(TMP, T, B);
VcrossV(VEC, B, TMP);
}
else {
VpVxS(X, P, A, t);
VcrossV(VEC, A, B);
if (VdotV(VEC, T) < 0) {
VxS(VEC, VEC, -1);
}
}
}
}
PQP_REAL
TriDist(PQP_REAL P[3], PQP_REAL Q[3],
const PQP_REAL S[3][3], const PQP_REAL T[3][3])
{
// Compute vectors along the 6 sides
PQP_REAL Sv[3][3], Tv[3][3];
PQP_REAL VEC[3];
VmV(Sv[0],S[1],S[0]);
VmV(Sv[1],S[2],S[1]);
VmV(Sv[2],S[0],S[2]);
VmV(Tv[0],T[1],T[0]);
VmV(Tv[1],T[2],T[1]);
VmV(Tv[2],T[0],T[2]);
// For each edge pair, the vector connecting the closest points
// of the edges defines a slab (parallel planes at head and tail
// enclose the slab). If we can show that the off-edge vertex of
// each triangle is outside of the slab, then the closest points
// of the edges are the closest points for the triangles.
// Even if these tests fail, it may be helpful to know the closest
// points found, and whether the triangles were shown disjoint
PQP_REAL V[3];
PQP_REAL Z[3];
PQP_REAL minP[3], minQ[3], mindd;
int shown_disjoint = 0;
mindd = VdistV2(S[0],T[0]) + 1; // Set first minimum safely high
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
// Find closest points on edges i & j, plus the
// vector (and distance squared) between these points
SegPoints(VEC,P,Q,S[i],Sv[i],T[j],Tv[j]);
VmV(V,Q,P);
PQP_REAL dd = VdotV(V,V);
// Verify this closest point pair only if the distance
// squared is less than the minimum found thus far.
if (dd <= mindd)
{
VcV(minP,P);
VcV(minQ,Q);
mindd = dd;
VmV(Z,S[(i+2)%3],P);
PQP_REAL a = VdotV(Z,VEC);
VmV(Z,T[(j+2)%3],Q);
PQP_REAL b = VdotV(Z,VEC);
if ((a <= 0) && (b >= 0)) return sqrt(dd);
PQP_REAL p = VdotV(V, VEC);
if (a < 0) a = 0;
if (b > 0) b = 0;
if ((p - a + b) > 0) shown_disjoint = 1;
}
}
}
// No edge pairs contained the closest points.
// either:
// 1. one of the closest points is a vertex, and the
// other point is interior to a face.
// 2. the triangles are overlapping.
// 3. an edge of one triangle is parallel to the other's face. If
// cases 1 and 2 are not true, then the closest points from the 9
// edge pairs checks above can be taken as closest points for the
// triangles.
// 4. possibly, the triangles were degenerate. When the
// triangle points are nearly colinear or coincident, one
// of above tests might fail even though the edges tested
// contain the closest points.
// First check for case 1
PQP_REAL Sn[3], Snl;
VcrossV(Sn,Sv[0],Sv[1]); // Compute normal to S triangle
Snl = VdotV(Sn,Sn); // Compute square of length of normal
// If cross product is long enough,
if (Snl > 1e-15)
{
// Get projection lengths of T points
PQP_REAL Tp[3];
VmV(V,S[0],T[0]);
Tp[0] = VdotV(V,Sn);
VmV(V,S[0],T[1]);
Tp[1] = VdotV(V,Sn);
VmV(V,S[0],T[2]);
Tp[2] = VdotV(V,Sn);
// If Sn is a separating direction,
// find point with smallest projection
int point = -1;
if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
{
if (Tp[0] < Tp[1]) point = 0; else point = 1;
if (Tp[2] < Tp[point]) point = 2;
}
else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
{
if (Tp[0] > Tp[1]) point = 0; else point = 1;
if (Tp[2] > Tp[point]) point = 2;
}
// If Sn is a separating direction,
if (point >= 0)
{
shown_disjoint = 1;
// Test whether the point found, when projected onto the
// other triangle, lies within the face.
VmV(V,T[point],S[0]);
VcrossV(Z,Sn,Sv[0]);
if (VdotV(V,Z) > 0)
{
VmV(V,T[point],S[1]);
VcrossV(Z,Sn,Sv[1]);
if (VdotV(V,Z) > 0)
{
VmV(V,T[point],S[2]);
VcrossV(Z,Sn,Sv[2]);
if (VdotV(V,Z) > 0)
{
// T[point] passed the test - it's a closest point for
// the T triangle; the other point is on the face of S
VpVxS(P,T[point],Sn,Tp[point]/Snl);
VcV(Q,T[point]);
return sqrt(VdistV2(P,Q));
}
}
}
}
}
PQP_REAL Tn[3], Tnl;
VcrossV(Tn,Tv[0],Tv[1]);
Tnl = VdotV(Tn,Tn);
if (Tnl > 1e-15)
{
PQP_REAL Sp[3];
VmV(V,T[0],S[0]);
Sp[0] = VdotV(V,Tn);
VmV(V,T[0],S[1]);
Sp[1] = VdotV(V,Tn);
VmV(V,T[0],S[2]);
Sp[2] = VdotV(V,Tn);
int point = -1;
if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
{
if (Sp[0] < Sp[1]) point = 0; else point = 1;
if (Sp[2] < Sp[point]) point = 2;
}
else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
{
if (Sp[0] > Sp[1]) point = 0; else point = 1;
if (Sp[2] > Sp[point]) point = 2;
}
if (point >= 0)
{
shown_disjoint = 1;
VmV(V,S[point],T[0]);
VcrossV(Z,Tn,Tv[0]);
if (VdotV(V,Z) > 0)
{
VmV(V,S[point],T[1]);
VcrossV(Z,Tn,Tv[1]);
if (VdotV(V,Z) > 0)
{
VmV(V,S[point],T[2]);
VcrossV(Z,Tn,Tv[2]);
if (VdotV(V,Z) > 0)
{
VcV(P,S[point]);
VpVxS(Q,S[point],Tn,Sp[point]/Tnl);
return sqrt(VdistV2(P,Q));
}
}
}
}
}
// Case 1 can't be shown.
// If one of these tests showed the triangles disjoint,
// we assume case 3 or 4, otherwise we conclude case 2,
// that the triangles overlap.
if (shown_disjoint)
{
VcV(P,minP);
VcV(Q,minQ);
return sqrt(mindd);
}
else return 0;
}
int
box::split_recurse(int *t)
{
// For a single triangle, orientation is easily determined.
// The major axis is parallel to the longest edge.
// The minor axis is normal to the triangle.
// The in-between axis is determine by these two.
// this->pR, this->d, and this->pT are set herein.
P = N = 0;
tri *ptr = RAPID_tri + t[0];
// Find the major axis: parallel to the longest edge.
double u12[3], u23[3], u31[3];
// First compute the squared-lengths of each edge
VmV(u12, ptr->p1, ptr->p2);
double d12 = VdotV(u12,u12);
VmV(u23, ptr->p2, ptr->p3);
double d23 = VdotV(u23,u23);
VmV(u31, ptr->p3, ptr->p1);
double d31 = VdotV(u31,u31);
// Find the edge of longest squared-length, normalize it to
// unit length, and put result into a0.
double a0[3];
double l;
if (d12 > d23)
{
if (d12 > d31)
{
l = 1.0 / sqrt(d12);
a0[0] = u12[0] * l;
a0[1] = u12[1] * l;
a0[2] = u12[2] * l;
}
else
{
l = 1.0 / sqrt(d31);
a0[0] = u31[0] * l;
a0[1] = u31[1] * l;
a0[2] = u31[2] * l;
}
}
else
{
if (d23 > d31)
{
l = 1.0 / sqrt(d23);
a0[0] = u23[0] * l;
a0[1] = u23[1] * l;
a0[2] = u23[2] * l;
}
else
{
l = 1.0 / sqrt(d31);
a0[0] = u31[0] * l;
a0[1] = u31[1] * l;
a0[2] = u31[2] * l;
}
}
// Now compute unit normal to triangle, and put into a2.
double a2[3];
VcrossV(a2, u12, u23);
l = 1.0 / Vlength(a2); a2[0] *= l; a2[1] *= l; a2[2] *= l;
// a1 is a2 cross a0.
double a1[3];
VcrossV(a1, a2, a0);
// Now make the columns of this->pR the vectors a0, a1, and a2.
pR[0][0] = a0[0]; pR[0][1] = a1[0]; pR[0][2] = a2[0];
pR[1][0] = a0[1]; pR[1][1] = a1[1]; pR[1][2] = a2[1];
pR[2][0] = a0[2]; pR[2][1] = a1[2]; pR[2][2] = a2[2];
// Now compute the maximum and minimum extents of each vertex
// along each of the box axes. From this we will compute the
// box center and box dimensions.
double minval[3], maxval[3];
double c[3];
MTxV(c, pR, ptr->p1);
minval[0] = maxval[0] = c[0];
minval[1] = maxval[1] = c[1];
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]);
// With the max and min data, determine the center point and dimensions
// of the 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;
// Assign the one triangle to this box
trp = ptr;
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;
}