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;
}
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);
}
}
}
}
