inline
void
compute_moment(moment &M, PQP_REAL p[3], PQP_REAL q[3], PQP_REAL r[3])
{
PQP_REAL u[3], v[3], w[3];
// compute the area of the triangle
VmV(u, q, p);
VmV(v, r, p);
VcrossV(w, u, v);
M.A = 0.5 * Vlength(w);
if (M.A == 0.0)
{
// This triangle has zero area. The second order components
// would be eliminated with the usual formula, so, for the
// sake of robustness we use an alternative form. These are the
// centroid and second-order components of the triangle's vertices.
// centroid
M.m[0] = (p[0] + q[0] + r[0]) /3;
M.m[1] = (p[1] + q[1] + r[1]) /3;
M.m[2] = (p[2] + q[2] + r[2]) /3;
// second-order components
M.s[0][0] = (p[0]*p[0] + q[0]*q[0] + r[0]*r[0]);
M.s[0][1] = (p[0]*p[1] + q[0]*q[1] + r[0]*r[1]);
M.s[0][2] = (p[0]*p[2] + q[0]*q[2] + r[0]*r[2]);
M.s[1][1] = (p[1]*p[1] + q[1]*q[1] + r[1]*r[1]);
M.s[1][2] = (p[1]*p[2] + q[1]*q[2] + r[1]*r[2]);
M.s[2][2] = (p[2]*p[2] + q[2]*q[2] + r[2]*r[2]);
M.s[2][1] = M.s[1][2];
M.s[1][0] = M.s[0][1];
M.s[2][0] = M.s[0][2];
return;
}
// get the centroid
M.m[0] = (p[0] + q[0] + r[0])/3;
M.m[1] = (p[1] + q[1] + r[1])/3;
M.m[2] = (p[2] + q[2] + r[2])/3;
// get the second order components -- note the weighting by the area
M.s[0][0] = M.A*(9*M.m[0]*M.m[0]+p[0]*p[0]+q[0]*q[0]+r[0]*r[0])/12;
M.s[0][1] = M.A*(9*M.m[0]*M.m[1]+p[0]*p[1]+q[0]*q[1]+r[0]*r[1])/12;
M.s[1][1] = M.A*(9*M.m[1]*M.m[1]+p[1]*p[1]+q[1]*q[1]+r[1]*r[1])/12;
M.s[0][2] = M.A*(9*M.m[0]*M.m[2]+p[0]*p[2]+q[0]*q[2]+r[0]*r[2])/12;
M.s[1][2] = M.A*(9*M.m[1]*M.m[2]+p[1]*p[2]+q[1]*q[2]+r[1]*r[2])/12;
M.s[2][2] = M.A*(9*M.m[2]*M.m[2]+p[2]*p[2]+q[2]*q[2]+r[2]*r[2])/12;
M.s[2][1] = M.s[1][2];
M.s[1][0] = M.s[0][1];
M.s[2][0] = M.s[0][2];
}
//sets all the members of Tri3B given the values of the vertices
void Tri3B::Set( const PQP_REAL *pt1, const PQP_REAL *pt2,
const PQP_REAL *pt3, int iden )
{
id = iden;
//vertices of triangle in model's coord. frame
p1[0] = pt1[0];
p1[1] = pt1[1];
p1[2] = pt1[2];
p2[0] = pt2[0];
p2[1] = pt2[1];
p2[2] = pt2[2];
p3[0] = pt3[0];
p3[1] = pt3[1];
p3[2] = pt3[2];
//normalized vectors along side of triangle
VmV( s12, p2, p1 );
Vnormalize( s12 );
VmV( s23, p3, p2 );
Vnormalize( s23 );
VmV( s31, p1, p3 );
Vnormalize( s31 );
//normal vector to plane of triangle
VcrossV( n, s31, s12 );
Vnormalize( n );
//normal vectors to each side in the plane of the triangle
//from cross product of side-vector and normal
VcrossV( ns12, s12, n );
Vnormalize( ns12 );
VcrossV( ns23, s23, n );
Vnormalize( ns23 );
VcrossV( ns31, s31, n );
Vnormalize( ns31 );
}
Model::Model(char *tris_file)
{
FILE *fp = fopen(tris_file,"r");
if (fp == NULL)
{
fprintf(stderr,"Model Constructor: Couldn't open %s\n",tris_file);
exit(-1);
}
fscanf(fp,"%d",&ntris);
tri = new ModelTri[ntris];
int i;
for (i = 0; i < ntris; i++)
{
// read the tri verts
fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf",
&tri[i].p0[0], &tri[i].p0[1], &tri[i].p0[2],
&tri[i].p1[0], &tri[i].p1[1], &tri[i].p1[2],
&tri[i].p2[0], &tri[i].p2[1], &tri[i].p2[2]);
// set the normal
double a[3],b[3];
VmV(a,tri[i].p1,tri[i].p0);
VmV(b,tri[i].p2,tri[i].p0);
VcrossV(tri[i].n,a,b);
Vnormalize(tri[i].n);
}
fclose(fp);
// generate display list
display_list = glGenLists(1);
glNewList(display_list,GL_COMPILE);
glBegin(GL_TRIANGLES);
for (i = 0; i < ntris; i++)
{
glNormal3dv(tri[i].n);
glVertex3dv(tri[i].p0);
glVertex3dv(tri[i].p1);
glVertex3dv(tri[i].p2);
}
glEnd();
glEndList();
}
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;
}
// very robust triangle intersection test
// uses no divisions
// works on coplanar triangles
int
TriContact(PQP_REAL *P1, PQP_REAL *P2, PQP_REAL *P3,
PQP_REAL *Q1, PQP_REAL *Q2, PQP_REAL *Q3)
{
// One triangle is (p1,p2,p3). Other is (q1,q2,q3).
// Edges are (e1,e2,e3) and (f1,f2,f3).
// Normals are n1 and m1
// Outwards are (g1,g2,g3) and (h1,h2,h3).
//
// We assume that the triangle vertices are in the same coordinate system.
//
// First thing we do is establish a new c.s. so that p1 is at (0,0,0).
PQP_REAL p1[3], p2[3], p3[3];
PQP_REAL q1[3], q2[3], q3[3];
PQP_REAL e1[3], e2[3], e3[3];
PQP_REAL f1[3], f2[3], f3[3];
PQP_REAL g1[3], g2[3], g3[3];
PQP_REAL h1[3], h2[3], h3[3];
PQP_REAL n1[3], m1[3];
PQP_REAL ef11[3], ef12[3], ef13[3];
PQP_REAL ef21[3], ef22[3], ef23[3];
PQP_REAL ef31[3], ef32[3], ef33[3];
p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2];
p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2];
p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2];
q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2];
q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2];
q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2];
e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2];
e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2];
e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2];
f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2];
f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2];
f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2];
VcrossV(n1, e1, e2);
VcrossV(m1, f1, f2);
VcrossV(g1, e1, n1);
VcrossV(g2, e2, n1);
VcrossV(g3, e3, n1);
VcrossV(h1, f1, m1);
VcrossV(h2, f2, m1);
VcrossV(h3, f3, m1);
VcrossV(ef11, e1, f1);
VcrossV(ef12, e1, f2);
VcrossV(ef13, e1, f3);
VcrossV(ef21, e2, f1);
VcrossV(ef22, e2, f2);
VcrossV(ef23, e2, f3);
VcrossV(ef31, e3, f1);
VcrossV(ef32, e3, f2);
VcrossV(ef33, e3, f3);
// now begin the series of tests
if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
return 1;
}
int rapidCollide::tri_contact (const float *P1, const float *P2, const float *P3,
const float *Q1, const float *Q2, const float *Q3)
{
/*
One triangle is (p1,p2,p3). Other is (q1,q2,q3).
Edges are (e1,e2,e3) and (f1,f2,f3).
Normals are n1 and m1
Outwards are (g1,g2,g3) and (h1,h2,h3).
We assume that the triangle vertices are in the same coordinate system.
First thing we do is establish a new c.s. so that p1 is at (0,0,0).
*/
float p1[3], p2[3], p3[3];
float q1[3], q2[3], q3[3];
float e1[3], e2[3], e3[3];
float f1[3], f2[3], f3[3];
float g1[3], g2[3], g3[3];
float h1[3], h2[3], h3[3];
float n1[3], m1[3];
float z[3];
float ef11[3], ef12[3], ef13[3];
float ef21[3], ef22[3], ef23[3];
float ef31[3], ef32[3], ef33[3];
z[0] = 0.0; z[1] = 0.0; z[2] = 0.0;
p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2];
p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2];
p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2];
q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2];
q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2];
q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2];
e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2];
e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2];
e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2];
f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2];
f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2];
f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2];
VcrossV(n1, e1, e2);
VcrossV(m1, f1, f2);
VcrossV(g1, e1, n1);
VcrossV(g2, e2, n1);
VcrossV(g3, e3, n1);
VcrossV(h1, f1, m1);
VcrossV(h2, f2, m1);
VcrossV(h3, f3, m1);
VcrossV(ef11, e1, f1);
VcrossV(ef12, e1, f2);
VcrossV(ef13, e1, f3);
VcrossV(ef21, e2, f1);
VcrossV(ef22, e2, f2);
VcrossV(ef23, e2, f3);
VcrossV(ef31, e3, f1);
VcrossV(ef32, e3, f2);
VcrossV(ef33, e3, f3);
// now begin the series of tests
if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
return 1;
}
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;
}
