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