int PointInPolyhedron(const Real p[restrict], const Polyhedron *poly, int fid[restrict])
{
const Real zero = 0.0;
RealVec v0 = {zero}; /* vertices */
RealVec v1 = {zero};
RealVec v2 = {zero};
RealVec e01 = {zero}; /* edges */
RealVec e02 = {zero};
RealVec pi = {zero}; /* closest point */
RealVec N = {zero}; /* normal of the closest point */
/*
* Parametric equation of triangle defined plane
* T(s,t) = v0 + s(v1-v0) + t(v2-v0) = v0 + s*e01 + t*e02
* s, t: real numbers. v0, v1, v2: vertices. e01, e02: edge vectors.
* A point pi = T(s,t) is in the triangle T when s>=0, t>=0, and s+t<=1.
* Further, pi is on an edge of T if one of the conditions s=0; t=0;
* s+t=1 is true with each condition corresponds to one edge. Each
* s=0, t=0; s=1, t=0; s=0, t=1 corresponds to v0, v1, and v2.
*/
RealVec para = {zero}; /* parametric coordinates */
Real distSquare = zero; /* store computed squared distance */
Real distSquareMin = FLT_MAX; /* store minimum squared distance */
int cid = 0; /* closest face identifier */
for (int n = 0; n < poly->faceN; ++n) {
BuildTriangle(n, poly, v0, v1, v2, e01, e02);
distSquare = PointTriangleDistance(p, v0, e01, e02, para);
if (distSquareMin > distSquare) {
distSquareMin = distSquare;
cid = n;
}
}
*fid = cid;
ComputeIntersection(p, cid, poly, pi, N);
pi[X] = p[X] - pi[X];
pi[Y] = p[Y] - pi[Y];
pi[Z] = p[Z] - pi[Z];
if (zero < Dot(pi, N)) {
/* outside polyhedron */
return 0;
} else {
/* inside or on polyhedron */
return 1;
}
}
Real ComputeIntersection(const Real p[restrict], const int fid, const Polyhedron *poly,
Real pi[restrict], Real N[restrict])
{
const Real zero = 0.0;
const Real one = 1.0;
RealVec v0 = {zero}; /* vertices */
RealVec v1 = {zero};
RealVec v2 = {zero};
RealVec e01 = {zero}; /* edges */
RealVec e02 = {zero};
RealVec para = {zero}; /* parametric coordinates */
const IntVec v = {poly->f[fid][0], poly->f[fid][1], poly->f[fid][2]}; /* vertex index in vertex list */
int e = 0; /* edge index in edge list */
BuildTriangle(fid, poly, v0, v1, v2, e01, e02);
const Real distSquare = PointTriangleDistance(p, v0, e01, e02, para);
if (zero == para[1]) {
if (zero == para[2]) {
/* vertex 0 */
for (int s = 0; s < DIMS; ++s) {
pi[s] = v0[s];
N[s] = poly->Nv[v[0]][s];
}
} else {
if (one == para[2]) {
/* vertex 2 */
for (int s = 0; s < DIMS; ++s) {
pi[s] = v2[s];
N[s] = poly->Nv[v[2]][s];
}
} else {
/* edge e02 */
e = FindEdge(v[0], v[2], poly->edgeN, poly->e);
for (int s = 0; s < DIMS; ++s) {
pi[s] = v0[s] + para[2] * e02[s];
N[s] = poly->Ne[e][s];
}
}
}
} else {
if (one == para[1]) {
/* vertex 1 */
for (int s = 0; s < DIMS; ++s) {
pi[s] = v1[s];
N[s] = poly->Nv[v[1]][s];
}
} else {
if (zero == para[2]) {
/* edge e01 */
e = FindEdge(v[0], v[1], poly->edgeN, poly->e);
for (int s = 0; s < DIMS; ++s) {
pi[s] = v0[s] + para[1] * e01[s];
N[s] = poly->Ne[e][s];
}
} else {
if (zero == para[0]) {
/* edge e12 */
e = FindEdge(v[1], v[2], poly->edgeN, poly->e);
for (int s = 0; s < DIMS; ++s) {
pi[s] = v0[s] + para[1] * e01[s] + para[2] * e02[s];
N[s] = poly->Ne[e][s];
}
} else {
/* complete in the triangle */
for (int s = 0; s < DIMS; ++s) {
pi[s] = v0[s] + para[1] * e01[s] + para[2] * e02[s];
N[s] = poly->Nf[fid][s];
}
}
}
}
}
return distSquare;
}
static void ComputeParametersPolyhedron(const int collapse, Polyhedron *poly)
{
/* initialize parameters */
const Real pi = 3.14159265359;
RealVec v0 = {0.0}; /* vertices */
RealVec v1 = {0.0};
RealVec v2 = {0.0};
RealVec e01 = {0.0}; /* edges */
RealVec e02 = {0.0};
RealVec Nf = {0.0}; /* outward normal */
RealVec Angle = {0.0}; /* internal angle */
RealVec O = {0.0}; /* centroid */
Real area = 0.0; /* area */
Real volume = 0.0; /* volume */
Real I[6] = {0.0}; /* inertia integration xx, yy, zz, xy, yz, zx */
RealVec tmp = {0.0}; /* temporary */
Real f[DIMS][DIMS] = {{0.0}}; /* temporary */
Real g[DIMS][DIMS] = {{0.0}}; /* temporary */
Real box[LIMIT][DIMS] = {{0.0}}; /* bounding box */
for (int s = 0; s < DIMS; ++s) {
box[MIN][s] = FLT_MAX;
box[MAX][s] = FLT_MIN;
}
/* initialize vertices normal */
memset(poly->Nv, 0, poly->vertN * sizeof(*poly->Nv));
/* bounding box */
for (int n = 0; n < poly->vertN; ++n) {
for (int s = 0; s < DIMS; ++s) {
box[MIN][s] = (box[MIN][s] < poly->v[n][s]) ? box[MIN][s] : poly->v[n][s];
box[MAX][s] = (box[MAX][s] > poly->v[n][s]) ? box[MAX][s] : poly->v[n][s];
}
}
/*
* Gelder, A. V. (1995). Efficient computation of polygon area and
* polyhedron volume. Graphics Gems V.
* Eberly, David. "Polyhedral mass properties (revisited)." Geometric
* Tools, LLC, Tech. Rep (2002).
*/
for (int n = 0; n < poly->faceN; ++n) {
BuildTriangle(n, poly, v0, v1, v2, e01, e02);
/* outward normal vector */
Cross(e01, e02, Nf);
/* temporary values */
for (int s = 0; s < DIMS; ++s) {
tmp[0] = v0[s] + v1[s];
f[0][s] = tmp[0] + v2[s];
tmp[1] = v0[s] * v0[s];
tmp[2] = tmp[1] + v1[s] * tmp[0];
f[1][s] = tmp[2] + v2[s] * f[0][s];
f[2][s] = v0[s] * tmp[1] + v1[s] * tmp[2] + v2[s] * f[1][s];
g[0][s] = f[1][s] + v0[s] * (f[0][s] + v0[s]);
g[1][s] = f[1][s] + v1[s] * (f[0][s] + v1[s]);
g[2][s] = f[1][s] + v2[s] * (f[0][s] + v2[s]);
}
/* integration */
area = area + Norm(Nf);
volume = volume + Nf[X] * f[0][X];
O[X] = O[X] + Nf[X] * f[1][X];
O[Y] = O[Y] + Nf[Y] * f[1][Y];
O[Z] = O[Z] + Nf[Z] * f[1][Z];
I[0] = I[0] + Nf[X] * f[2][X];
I[1] = I[1] + Nf[Y] * f[2][Y];
I[2] = I[2] + Nf[Z] * f[2][Z];
I[3] = I[3] + Nf[X] * (v0[Y] * g[0][X] + v1[Y] * g[1][X] + v2[Y] * g[2][X]);
I[4] = I[4] + Nf[Y] * (v0[Z] * g[0][Y] + v1[Z] * g[1][Y] + v2[Z] * g[2][Y]);
I[5] = I[5] + Nf[Z] * (v0[X] * g[0][Z] + v1[X] * g[1][Z] + v2[X] * g[2][Z]);
/* unit normal */
Normalize(DIMS, Norm(Nf), Nf);
/*
* Refine vertices normal by corresponding angles
* Baerentzen, J. A., & Aanaes, H. (2005). Signed distance computation
* using the angle weighted pseudonormal. Visualization and Computer
* Graphics, IEEE Transactions on, 11(3), 243-253.
*/
/* calculate internal angles by the law of cosines */
const RealVec e12 = {v2[X] - v1[X], v2[Y] - v1[Y], v2[Z] - v1[Z]};
const RealVec lsq = {Dot(e01, e01), Dot(e02, e02), Dot(e12, e12)};
Angle[0] = acos((lsq[0] + lsq[1] - lsq[2]) / (2.0 * sqrt(lsq[0] * lsq[1])));
Angle[1] = acos((lsq[0] + lsq[2] - lsq[1]) / (2.0 * sqrt(lsq[0] * lsq[2])));
Angle[2] = pi - Angle[0] - Angle[1];
for (int v = 0; v < POLYN; ++v) {
for (int s = 0; s < DIMS; ++s) {
poly->Nv[poly->f[n][v]][s] = poly->Nv[poly->f[n][v]][s] + Angle[v] * Nf[s];
}
}
/* assign face normal */
for (int s = 0; s < DIMS; ++s) {
poly->Nf[n][s] = Nf[s];
}
}
/* rectify final integration */
area = area * (1.0 / 2.0);
volume = volume * (1.0 / 6.0);
O[X] = O[X] * (1.0 / 24.0);
O[Y] = O[Y] * (1.0 / 24.0);
O[Z] = O[Z] * (1.0 / 24.0);
I[0] = I[0] * (1.0 / 60.0);
I[1] = I[1] * (1.0 / 60.0);
I[2] = I[2] * (1.0 / 60.0);
I[3] = I[3] * (1.0 / 120.0);
I[4] = I[4] * (1.0 / 120.0);
I[5] = I[5] * (1.0 / 120.0);
O[X] = O[X] / volume;
O[Y] = O[Y] / volume;
O[Z] = O[Z] / volume;
/* assign to polyhedron */
if (COLLAPSEN == collapse) { /* no space dimension collapsed */
poly->area = area;
} else {
poly->area = area - 2.0 * volume; /* change to side area of a unit thickness polygon */
}
poly->volume = volume;
poly->O[X] = O[X];
poly->O[Y] = O[Y];
poly->O[Z] = O[Z];
/* inertia relative to centroid */
poly->I[X][X] = I[1] + I[2] - volume * (O[Y] * O[Y] + O[Z] * O[Z]);
poly->I[X][Y] = -I[3] + volume * O[X] * O[Y];
poly->I[X][Z] = -I[5] + volume * O[Z] * O[X];
poly->I[Y][X] = poly->I[X][Y];
poly->I[Y][Y] = I[0] + I[2] - volume * (O[Z] * O[Z] + O[X] * O[X]);
poly->I[Y][Z] = -I[4] + volume * O[Y] * O[Z];
poly->I[Z][X] = poly->I[X][Z];
poly->I[Z][Y] = poly->I[Y][Z];
poly->I[Z][Z] = I[0] + I[1] - volume * (O[X] * O[X] + O[Y] * O[Y]);
for (int s = 0; s < DIMS; ++s) {
poly->box[s][MIN] = box[MIN][s];
poly->box[s][MAX] = box[MAX][s];
}
/* a radius for estimating maximum velocity */
poly->r = Dist(box[MIN], box[MAX]);
/* normalize vertices normal */
for (int n = 0; n < poly->vertN; ++n) {
Normalize(DIMS, Norm(poly->Nv[n]), poly->Nv[n]);
}
/* compute edge normal */
for (int n = 0; n < poly->edgeN; ++n) {
for (int s = 0; s < DIMS; ++s) {
poly->Ne[n][s] = poly->Nf[poly->e[n][2]][s] + poly->Nf[poly->e[n][3]][s];
}
Normalize(DIMS, Norm(poly->Ne[n]), poly->Ne[n]);
}
return;
}
//
// Override the abstract Create function to make this
// a real class.
// Create works its way through the text in the scanner one
// line at a time. Each line must contain a command and a set
// of arguments. Create examines the command and then calls
// on helper functions to do the right things with the arguments.
//
bool CGLAList::Create() {
if (mScan == NULL) { // Quit if there is no scanner
return false;
}
//
// Work through the file.
// Each line should begin with a command and then be
// followed by a set of numeric arguments.
//
Lexeme* cLex;
CSymbol* cSym = NULL;
bool finished = false;
Start();
while ((cLex = mScan->NextLex())->lex != LEof) {
if (cLex->lex == LNewLine || cLex->lex==LSpace) continue;
if (cLex->lex == LWord) { // Hope we have a command
#ifdef DEBUG
eprintf("Word %s ", cLex->sVal);
wxLogMessage(gMsgBuff);
#endif
cSym = gSymTab->LookUpWord(cLex->sVal);
//
// Now read in the arguments.
//
int nArg = GetArgList();
#ifdef DEBUG
eprintf("Found %d arguments",nArg);
wxLogMessage(gMsgBuff);
for (int ii = 0; ii < nArg; ++ii) {
eprintf(" %f", gArguments[ii]);
wxLogMessage(gMsgBuff);
}
wxLogMessage("\r\n");
#endif
if (cSym != NULL) {
//
// Found a valid command, dispatch a function
// to handle it.
//
#ifdef DEBUG
eprintf("matches symbol %d\r\n",cSym->mSym);
wxLogMessage(gMsgBuff);
#endif
switch (cSym->mSym) {
case kGLColor:
if (nArg >= 3) {
GLfloat color[4];
color[0] = gArguments[0];
color[1] = gArguments[1];
color[2] = gArguments[2];
if (nArg > 3) {
color[3] = gArguments[3];
} else {
color[3] = 1.0f;
}
glMaterialfv(GL_FRONT, GL_AMBIENT, color);
glMaterialfv(GL_FRONT, GL_SPECULAR, color);
glColor3fv(color);
}
break;
case kGLPoint:
if (nArg >= 3) {
glBegin(GL_POINTS);
glVertex3fv(gArguments);
glEnd();
mBounds->AddPoint3fv(&gArguments[0]);
}
break;
case kGLTranslate:
if (nArg >= 3) {
glTranslatef(gArguments[0],gArguments[1],gArguments[2]);
}
break;
case kGLLine:
BuildLine(nArg);
break;
case kGLPolyLine:
BuildPolyLine(nArg);
break;
case kGLSphere:
BuildSphere(nArg);
break;
case kGLBox:
BuildBox(nArg);
break;
case kGLTriangle:
BuildTriangle(nArg);
break;
case kGLCylinder:
BuildCylinder(nArg);
break;
case kGLCap:
BuildCap(nArg);
break;
case kGLEnd:
finished = true;
break;
default:
eprintf("CGLAList.Create: Unimplemented GLA verb %s.\r\n", cSym->mName);
break;
}
} else {
eprintf("CGLAList.Create: %s is an unrecognised GLA verb.\r\n", cLex->sVal);
}
} else { // Did not find a command
//
// Print a message.
//
eprintf("CGLAList.Create: Cound not find a command on line %lu.\r\n", mScan->LineNumber());
eprintf("%s", mScan->GetLine());
}
if (finished) break;
//
// Eat tokens til we get to the
// end of the line (or a premature EOF)
//
while ((cLex = mScan->NextLex())->lex != LNewLine && cLex->lex != LEof);
} // end while
End();
ReleaseScanner();
return true;
}
