diff --git a/src/meshTools/momentOfInertia/momentOfInertia.C b/src/meshTools/momentOfInertia/momentOfInertia.C new file mode 100644 index 0000000000..5207850172 --- /dev/null +++ b/src/meshTools/momentOfInertia/momentOfInertia.C @@ -0,0 +1,403 @@ +/*---------------------------------------------------------------------------*\ + ========= | + \\ / F ield | OpenFOAM: The Open Source CFD Toolbox + \\ / O peration | + \\ / A nd | Copyright (C) 2007-2009 OpenCFD Ltd. + \\/ M anipulation | +------------------------------------------------------------------------------- +License + This file is part of OpenFOAM. + + OpenFOAM is free software; you can redistribute it and/or modify it + under the terms of the GNU General Public License as published by the + Free Software Foundation; either version 2 of the License, or (at your + option) any later version. + + OpenFOAM is distributed in the hope that it will be useful, but WITHOUT + ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + for more details. + + You should have received a copy of the GNU General Public License + along with OpenFOAM; if not, write to the Free Software Foundation, + Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +Class + momentOfInertia + +Description + Reimplementation of volInt.c by Brian Mirtich. + * mirtich@cs.berkeley.edu * + * http://www.cs.berkeley.edu/~mirtich * + +------------------------------------------------------------------------------- +*/ + +#include "momentOfInertia.H" +//#include "pyramidPointFaceRef.H" + +// * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * // + +//Foam::tensor Foam::momentOfInertia +//( +// const pointField& points, +// const faceList& faces, +// const cell& cFaces, +// const point& cc +//) +//{ +// tensor t(tensor::zero); +// +// forAll(cFaces, i) +// { +// const face& f = faces[cFaces[i]]; +// +// scalar pyrVol = pyramidPointFaceRef(f, cc).mag(points); +// +// vector pyrCentre = pyramidPointFaceRef(f, cc).centre(points); +// +// vector d = pyrCentre - cc; +// +// t.xx() += pyrVol*(sqr(d.y()) + sqr(d.z())); +// t.yy() += pyrVol*(sqr(d.x()) + sqr(d.z())); +// t.zz() += pyrVol*(sqr(d.x()) + sqr(d.y())); +// +// t.xy() -= pyrVol*d.x()*d.y(); +// t.xz() -= pyrVol*d.x()*d.z(); +// t.yz() -= pyrVol*d.y()*d.z(); +// } +// +// // Symmetric +// t.yx() = t.xy(); +// t.zx() = t.xz(); +// t.zy() = t.yz(); +// +// return t; +//} + + +#define sqr(x) ((x)*(x)) +#define pow3(x) ((x)*(x)*(x)) + +// compute various integrations over projection of face +void Foam::compProjectionIntegrals +( + const pointField& points, + const face& f, + const direction A, + const direction B, + + scalar& P1, + scalar& Pa, + scalar& Pb, + scalar& Paa, + scalar& Pab, + scalar& Pbb, + scalar& Paaa, + scalar& Paab, + scalar& Pabb, + scalar& Pbbb +) +{ + P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; + + forAll(f, i) + { + scalar a0 = points[f[i]][A]; + scalar b0 = points[f[i]][B]; + scalar a1 = points[f[(i+1) % f.size()]][A]; + scalar b1 = points[f[(i+1) % f.size()]][B]; + scalar da = a1 - a0; + scalar db = b1 - b0; + + scalar a0_2 = a0 * a0; + scalar a0_3 = a0_2 * a0; + scalar a0_4 = a0_3 * a0; + + scalar b0_2 = b0 * b0; + scalar b0_3 = b0_2 * b0; + scalar b0_4 = b0_3 * b0; + + scalar a1_2 = a1 * a1; + scalar a1_3 = a1_2 * a1; + + scalar b1_2 = b1 * b1; + scalar b1_3 = b1_2 * b1; + + scalar C1 = a1 + a0; + + scalar Ca = a1*C1 + a0_2; + scalar Caa = a1*Ca + a0_3; + scalar Caaa = a1*Caa + a0_4; + + scalar Cb = b1*(b1 + b0) + b0_2; + scalar Cbb = b1*Cb + b0_3; + scalar Cbbb = b1*Cbb + b0_4; + + scalar Cab = 3*a1_2 + 2*a1*a0 + a0_2; + scalar Kab = a1_2 + 2*a1*a0 + 3*a0_2; + + scalar Caab = a0*Cab + 4*a1_3; + scalar Kaab = a1*Kab + 4*a0_3; + + scalar Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; + scalar Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; + + P1 += db*C1; + Pa += db*Ca; + Paa += db*Caa; + Paaa += db*Caaa; + Pb += da*Cb; + Pbb += da*Cbb; + Pbbb += da*Cbbb; + Pab += db*(b1*Cab + b0*Kab); + Paab += db*(b1*Caab + b0*Kaab); + Pabb += da*(a1*Cabb + a0*Kabb); + } + + P1 /= 2.0; + Pa /= 6.0; + Paa /= 12.0; + Paaa /= 20.0; + Pb /= -6.0; + Pbb /= -12.0; + Pbbb /= -20.0; + Pab /= 24.0; + Paab /= 60.0; + Pabb /= -60.0; +} + + +void Foam::compFaceIntegrals +( + const pointField& points, + const face& f, + const vector& n, + const scalar w, + const direction A, + const direction B, + const direction C, + + scalar& Fa, + scalar& Fb, + scalar& Fc, + scalar& Faa, + scalar& Fbb, + scalar& Fcc, + scalar& Faaa, + scalar& Fbbb, + scalar& Fccc, + scalar& Faab, + scalar& Fbbc, + scalar& Fcca +) +{ + scalar P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; + + compProjectionIntegrals + ( + points, + f, + A, + B, + + P1, + Pa, + Pb, + Paa, + Pab, + Pbb, + Paaa, + Paab, + Pabb, + Pbbb + ); + + scalar k1 = 1 / n[C]; + scalar k2 = k1 * k1; + scalar k3 = k2 * k1; + scalar k4 = k3 * k1; + + Fa = k1 * Pa; + Fb = k1 * Pb; + Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); + + Faa = k1 * Paa; + Fbb = k1 * Pbb; + Fcc = k3 * (sqr(n[A])*Paa + 2*n[A]*n[B]*Pab + sqr(n[B])*Pbb + + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faaa = k1 * Paaa; + Fbbb = k1 * Pbbb; + Fccc = -k4 * (pow3(n[A])*Paaa + 3*sqr(n[A])*n[B]*Paab + + 3*n[A]*sqr(n[B])*Pabb + pow3(n[B])*Pbbb + + 3*w*(sqr(n[A])*Paa + 2*n[A]*n[B]*Pab + sqr(n[B])*Pbb) + + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faab = k1 * Paab; + Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); + Fcca = k3 * (sqr(n[A])*Paaa + 2*n[A]*n[B]*Paab + sqr(n[B])*Pabb + + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); +} + + +void Foam::compVolumeIntegrals +( + const pointField& points, + const faceList& faces, + const cell& cFaces, + const vectorField& fNorm, + const scalarField& fW, + + scalar& T0, + vector& T1, + vector& T2, + vector& TP +) +{ + T0 = 0; + T1 = vector::zero; + T2 = vector::zero; + TP = vector::zero; + + forAll(cFaces, i) + { + const vector& n = fNorm[i]; + + scalar nx = mag(n[0]); + scalar ny = mag(n[1]); + scalar nz = mag(n[2]); + + direction A, B, C; + + if (nx > ny && nx > nz) + { + C = 0; + } + else + { + C = (ny > nz) ? 1 : 2; + } + + A = (C + 1) % 3; + B = (A + 1) % 3; + + scalar Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; + compFaceIntegrals + ( + points, + faces[cFaces[i]], + n, + fW[i], + A, + B, + C, + + Fa, + Fb, + Fc, + Faa, + Fbb, + Fcc, + Faaa, + Fbbb, + Fccc, + Faab, + Fbbc, + Fcca + ); + + T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc)); + + T1[A] += n[A] * Faa; + T1[B] += n[B] * Fbb; + T1[C] += n[C] * Fcc; + + T2[A] += n[A] * Faaa; + T2[B] += n[B] * Fbbb; + T2[C] += n[C] * Fccc; + + TP[A] += n[A] * Faab; + TP[B] += n[B] * Fbbc; + TP[C] += n[C] * Fcca; + } + + T1 /= 2; + T2 /= 3; + TP /= 2; +} + + +// Calculate +// - r: centre of mass +// - J: inertia around origin (point 0,0,0) +void Foam::momentOfIntertia +( + const pointField& points, + const faceList& faces, + const cell& cFaces, + point& r, + tensor& J +) +{ + // Face normals + vectorField fNorm(cFaces.size()); + scalarField fW(cFaces.size()); + + forAll(cFaces, i) + { + label faceI = cFaces[i]; + + const face& f = faces[faceI]; + + fNorm[i] = f.normal(points); + fNorm[i] /= mag(fNorm[i]) + VSMALL; + + fW[i] = - (fNorm[i] & points[f[0]]); + } + + + scalar T0; + vector T1, T2, TP; + + compVolumeIntegrals + ( + points, + faces, + cFaces, + fNorm, + fW, + + T0, + T1, + T2, + TP + ); + + const scalar density = 1.0; /* assume unit density */ + + scalar mass = density * T0; + + /* compute center of mass */ + r = T1 / T0; + + /* compute inertia tensor */ + J.xx() = density * (T2[1] + T2[2]); + J.yy() = density * (T2[2] + T2[0]); + J.zz() = density * (T2[0] + T2[1]); + J.xy() = J.yx() = - density * TP[0]; + J.yz() = J.zy() = - density * TP[1]; + J.zx() = J.xz() = - density * TP[2]; + + ///* translate inertia tensor to center of mass */ + //J[XX] -= mass * (r[1]*r[1] + r[2]*r[2]); + //J[YY] -= mass * (r[2]*r[2] + r[0]*r[0]); + //J[ZZ] -= mass * (r[0]*r[0] + r[1]*r[1]); + //J[XY] = J[YX] += mass * r[0] * r[1]; + //J[YZ] = J[ZY] += mass * r[1] * r[2]; + //J[ZX] = J[XZ] += mass * r[2] * r[0]; +} + + + +// ************************************************************************* // diff --git a/src/meshTools/momentOfInertia/momentOfInertia.H b/src/meshTools/momentOfInertia/momentOfInertia.H new file mode 100644 index 0000000000..66a8d357ad --- /dev/null +++ b/src/meshTools/momentOfInertia/momentOfInertia.H @@ -0,0 +1,75 @@ +/*---------------------------------------------------------------------------*\ + ========= | + \\ / F ield | OpenFOAM: The Open Source CFD Toolbox + \\ / O peration | + \\ / A nd | Copyright (C) 2007-2009 OpenCFD Ltd. + \\/ M anipulation | +------------------------------------------------------------------------------- +License + This file is part of OpenFOAM. + + OpenFOAM is free software; you can redistribute it and/or modify it + under the terms of the GNU General Public License as published by the + Free Software Foundation; either version 2 of the License, or (at your + option) any later version. + + OpenFOAM is distributed in the hope that it will be useful, but WITHOUT + ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + for more details. + + You should have received a copy of the GNU General Public License + along with OpenFOAM; if not, write to the Free Software Foundation, + Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +Class + momentOfInertia + +Description + +SourceFiles + momentOfInertia.H + +\*---------------------------------------------------------------------------*/ + +#ifndef momentOfInertia_H +#define momentOfInertia_H + +#include "tensor.H" +#include "primitiveMesh.H" + +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // + +namespace Foam +{ + +////- Moment of inertia around cell centre for single cell. +//tensor momentOfInertia +//( +// const pointField&, +// const faceList&, +// const cell&, +// const point& cc +//); + +// Calculate +// - centre of mass +// - inertia tensor around (0,0,0) +void momentOfIntertia +( + const pointField&, + const faceList&, + const cell&, + point& r, + tensor& Jorigin +); + +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // + +} // End namespace Foam + +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // + +#endif + +// ************************************************************************* // diff --git a/src/meshTools/momentOfInertia/volInt.ps.gz b/src/meshTools/momentOfInertia/volInt.ps.gz new file mode 100644 index 0000000000..19fb295421 Binary files /dev/null and b/src/meshTools/momentOfInertia/volInt.ps.gz differ diff --git a/src/meshTools/momentOfInertia/volumeIntegration/README b/src/meshTools/momentOfInertia/volumeIntegration/README new file mode 100644 index 0000000000..dadca677b7 --- /dev/null +++ b/src/meshTools/momentOfInertia/volumeIntegration/README @@ -0,0 +1,146 @@ +1. OVERVIEW + + This code accompanies the paper: + + Brian Mirtich, "Fast and Accurate Computation of + Polyhedral Mass Properties," journal of graphics + tools, volume 1, number 2, 1996. + + It computes the ten volume integrals needed for + determining the center of mass, moments of + inertia, and products of inertia for a uniform + density polyhedron. From this information, a + body frame can be computed. + + To compile the program, use an ANSI compiler, and + type something like + + % cc volInt.c -O2 -lm -o volInt + + + Revision history + + 26 Jan 1996 Program creation. + + 3 Aug 1996 Corrected bug arising when polyhedron density + is not 1.0. Changes confined to function main(). + Thanks to Zoran Popovic for catching this one. + + + +2. POLYHEDRON GEOMETRY FILES + + The program reads a data file specified on the + command line. This data file describes the + geometry of a polyhedron, and has the following + format: + + N + + x_0 y_0 z_0 + x_1 y_1 z_1 + . + . + . + x_{N-1} y_{N-1} z_{N-1} + + M + + k1 v_{1,1} v_{1,2} ... v_{1,k1} + k2 v_{2,1} v_{2,2} ... v_{2,k2} + . + . + . + kM v_{M,1} v_{M,2} ... v_{M,kM} + + + where: + N number of vertices on polyhedron + x_i y_i z_i x, y, and z coordinates of ith vertex + M number of faces on polyhedron + ki number of vertices on ith face + v_{i,j} jth vertex on ith face + + In English: + + First the number of vertices are specified. Next + the vertices are defined by listing the 3 + coordinates of each one. Next the number of faces + are specified. Finally, the faces themselves are + specified. A face is specified by first giving + the number of vertices around the polygonal face, + followed by the (integer) indices of these + vertices. When specifying indices, note that + they must be given in counter-clockwise order + (when looking at the face from outside the + polyhedron), and the vertices are indexed from 0 + to N-1 for a polyhedron with N faces. + + White space can be inserted (or not) as desired. + Three example polyhedron geometry files are included: + + cube A cube, 20 units on a side, centered at + the origin and aligned with the coordinate axes. + + tetra A tetrahedron formed by taking the convex + hull of the origin, and the points (5,0,0), + (0,4,0), and (0,0,3). + + icosa An icosahedron with vertices lying on the unit + sphere, centered at the origin. + + + +3. RUNNING THE PROGRAM + + Simply type, + + % volInt + + The program will read in the geometry of the + polyhedron, and the print out the ten volume + integrals. + + The program also computes some of the mass + properties which may be inferred from the volume + integrals. A density of 1.0 is assumed, although + this may be changed in function main(). The + center of mass is computed as well as the inertia + tensor relative to a frame with origin at the + center of mass. Note, however, that this matrix + may not be diagonal. If not, a diagonalization + routine must be performed, to determine the + orientation of the true body frame relative to + the original model frame. The Jacobi method + works quite well (see Numerical Recipes in C by + Press, et. al.). + + + +4. DISCLAIMERS + + 1. The volume integration code has been written + to match the development and algorithms presented + in the paper, and not with maximum optimization + in mind. While inherently very efficient, a few + more cycles can be squeezed out of the algorithm. + This is left as an exercise. :) + + 2. Don't like global variables? The three + procedures which evaluate the volume integrals + can be combined into a single procedure with two + nested loops. In addition to providing some + speedup, all of the global variables can then be + made local. + + 3. The polyhedron data structure used by the + program is admittedly lame; much better schemes + are possible. The idea here is just to give the + basic integral evaluation code, which will have + to be adjusted for other polyhedron data + structures. + + 4. There is no error checking for the input + files. Be careful. Note the hard limits + #defined for the number of vertices, number of + faces, and number of vertices per faces. diff --git a/src/meshTools/momentOfInertia/volumeIntegration/cube b/src/meshTools/momentOfInertia/volumeIntegration/cube new file mode 100644 index 0000000000..07ab26ad16 --- /dev/null +++ b/src/meshTools/momentOfInertia/volumeIntegration/cube @@ -0,0 +1,21 @@ +8 + +-10 -10 -10 ++10 -10 -10 ++10 +10 -10 +-10 +10 -10 +-10 -10 +10 ++10 -10 +10 ++10 +10 +10 +-10 +10 +10 + +6 + +4 0 3 2 1 +4 4 5 6 7 +4 0 1 5 4 +4 6 2 3 7 +4 1 2 6 5 +4 0 4 7 3 + + diff --git a/src/meshTools/momentOfInertia/volumeIntegration/icosa b/src/meshTools/momentOfInertia/volumeIntegration/icosa new file mode 100644 index 0000000000..16339ece89 --- /dev/null +++ b/src/meshTools/momentOfInertia/volumeIntegration/icosa @@ -0,0 +1,38 @@ +12 + ++0.00000000 +0.00000000 +1.00000000 ++0.00000000 +0.00000000 -1.00000000 ++0.89442719 +0.00000000 +0.44721360 ++0.27639320 +0.85065081 +0.44721360 +-0.72360680 +0.52573111 +0.44721360 +-0.72360680 -0.52573111 +0.44721360 ++0.27639320 -0.85065081 +0.44721360 ++0.72360680 +0.52573111 -0.44721360 +-0.27639320 +0.85065081 -0.44721360 +-0.89442719 +0.00000000 -0.44721360 +-0.27639320 -0.85065081 -0.44721360 ++0.72360680 -0.52573111 -0.44721360 + +20 + +3 6 11 2 +3 3 2 7 +3 7 2 11 +3 0 2 3 +3 0 6 2 +3 10 1 11 +3 1 7 11 +3 10 11 6 +3 8 7 1 +3 8 3 7 +3 5 10 6 +3 5 6 0 +3 4 3 8 +3 4 0 3 +3 9 8 1 +3 9 1 10 +3 4 5 0 +3 9 10 5 +3 9 5 4 +3 9 4 8 + diff --git a/src/meshTools/momentOfInertia/volumeIntegration/tetra b/src/meshTools/momentOfInertia/volumeIntegration/tetra new file mode 100644 index 0000000000..159e5a2bf8 --- /dev/null +++ b/src/meshTools/momentOfInertia/volumeIntegration/tetra @@ -0,0 +1,16 @@ +4 + +0 0 0 +5 0 0 +0 4 0 +0 0 3 + +4 + +3 0 3 2 +3 3 0 1 +3 2 1 0 +3 1 2 3 + + + diff --git a/src/meshTools/momentOfInertia/volumeIntegration/volInt.c b/src/meshTools/momentOfInertia/volumeIntegration/volInt.c new file mode 100644 index 0000000000..7601655488 --- /dev/null +++ b/src/meshTools/momentOfInertia/volumeIntegration/volInt.c @@ -0,0 +1,380 @@ + + + /******************************************************* + * * + * volInt.c * + * * + * This code computes volume integrals needed for * + * determining mass properties of polyhedral bodies. * + * * + * For more information, see the accompanying README * + * file, and the paper * + * * + * Brian Mirtich, "Fast and Accurate Computation of * + * Polyhedral Mass Properties," journal of graphics * + * tools, volume 1, number 1, 1996. * + * * + * This source code is public domain, and may be used * + * in any way, shape or form, free of charge. * + * * + * Copyright 1995 by Brian Mirtich * + * * + * mirtich@cs.berkeley.edu * + * http://www.cs.berkeley.edu/~mirtich * + * * + *******************************************************/ + +/* + Revision history + + 26 Jan 1996 Program creation. + + 3 Aug 1996 Corrected bug arising when polyhedron density + is not 1.0. Changes confined to function main(). + Thanks to Zoran Popovic for catching this one. + + 27 May 1997 Corrected sign error in translation of inertia + product terms to center of mass frame. Changes + confined to function main(). Thanks to + Chris Hecker. +*/ + + + +#include +#include +#include + +/* + ============================================================================ + constants + ============================================================================ +*/ + +#define MAX_VERTS 100 /* maximum number of polyhedral vertices */ +#define MAX_FACES 100 /* maximum number of polyhedral faces */ +#define MAX_POLYGON_SZ 10 /* maximum number of verts per polygonal face */ + +#define X 0 +#define Y 1 +#define Z 2 + +/* + ============================================================================ + macros + ============================================================================ +*/ + +#define SQR(x) ((x)*(x)) +#define CUBE(x) ((x)*(x)*(x)) + +/* + ============================================================================ + data structures + ============================================================================ +*/ + +typedef struct { + int numVerts; + double norm[3]; + double w; + int verts[MAX_POLYGON_SZ]; + struct polyhedron *poly; +} FACE; + +typedef struct polyhedron { + int numVerts, numFaces; + double verts[MAX_VERTS][3]; + FACE faces[MAX_FACES]; +} POLYHEDRON; + + +/* + ============================================================================ + globals + ============================================================================ +*/ + +static int A; /* alpha */ +static int B; /* beta */ +static int C; /* gamma */ + +/* projection integrals */ +static double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; + +/* face integrals */ +static double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; + +/* volume integrals */ +static double T0, T1[3], T2[3], TP[3]; + + +/* + ============================================================================ + read in a polyhedron + ============================================================================ +*/ + +void readPolyhedron(char *name, POLYHEDRON *p) +{ + FILE *fp; + char line[200], *c; + int i, j, n; + double dx1, dy1, dz1, dx2, dy2, dz2, nx, ny, nz, len; + FACE *f; + + + if (!(fp = fopen(name, "r"))) { + printf("i/o error\n"); + exit(1); + } + + fscanf(fp, "%d", &p->numVerts); + printf("Reading in %d vertices\n", p->numVerts); + for (i = 0; i < p->numVerts; i++) + fscanf(fp, "%lf %lf %lf", + &p->verts[i][X], &p->verts[i][Y], &p->verts[i][Z]); + + fscanf(fp, "%d", &p->numFaces); + printf("Reading in %d faces\n", p->numFaces); + for (i = 0; i < p->numFaces; i++) { + f = &p->faces[i]; + f->poly = p; + fscanf(fp, "%d", &f->numVerts); + for (j = 0; j < f->numVerts; j++) fscanf(fp, "%d", &f->verts[j]); + + /* compute face normal and offset w from first 3 vertices */ + dx1 = p->verts[f->verts[1]][X] - p->verts[f->verts[0]][X]; + dy1 = p->verts[f->verts[1]][Y] - p->verts[f->verts[0]][Y]; + dz1 = p->verts[f->verts[1]][Z] - p->verts[f->verts[0]][Z]; + dx2 = p->verts[f->verts[2]][X] - p->verts[f->verts[1]][X]; + dy2 = p->verts[f->verts[2]][Y] - p->verts[f->verts[1]][Y]; + dz2 = p->verts[f->verts[2]][Z] - p->verts[f->verts[1]][Z]; + nx = dy1 * dz2 - dy2 * dz1; + ny = dz1 * dx2 - dz2 * dx1; + nz = dx1 * dy2 - dx2 * dy1; + len = sqrt(nx * nx + ny * ny + nz * nz); + f->norm[X] = nx / len; + f->norm[Y] = ny / len; + f->norm[Z] = nz / len; + f->w = - f->norm[X] * p->verts[f->verts[0]][X] + - f->norm[Y] * p->verts[f->verts[0]][Y] + - f->norm[Z] * p->verts[f->verts[0]][Z]; + + } + + fclose(fp); + +} + +/* + ============================================================================ + compute mass properties + ============================================================================ +*/ + + +/* compute various integrations over projection of face */ +void compProjectionIntegrals(FACE *f) +{ + double a0, a1, da; + double b0, b1, db; + double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; + double a1_2, a1_3, b1_2, b1_3; + double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; + double Cab, Kab, Caab, Kaab, Cabb, Kabb; + int i; + + P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; + + for (i = 0; i < f->numVerts; i++) { + a0 = f->poly->verts[f->verts[i]][A]; + b0 = f->poly->verts[f->verts[i]][B]; + a1 = f->poly->verts[f->verts[(i+1) % f->numVerts]][A]; + b1 = f->poly->verts[f->verts[(i+1) % f->numVerts]][B]; + da = a1 - a0; + db = b1 - b0; + a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; + b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; + a1_2 = a1 * a1; a1_3 = a1_2 * a1; + b1_2 = b1 * b1; b1_3 = b1_2 * b1; + + C1 = a1 + a0; + Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; + Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; + Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; + Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; + Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; + Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; + + P1 += db*C1; + Pa += db*Ca; + Paa += db*Caa; + Paaa += db*Caaa; + Pb += da*Cb; + Pbb += da*Cbb; + Pbbb += da*Cbbb; + Pab += db*(b1*Cab + b0*Kab); + Paab += db*(b1*Caab + b0*Kaab); + Pabb += da*(a1*Cabb + a0*Kabb); + } + + P1 /= 2.0; + Pa /= 6.0; + Paa /= 12.0; + Paaa /= 20.0; + Pb /= -6.0; + Pbb /= -12.0; + Pbbb /= -20.0; + Pab /= 24.0; + Paab /= 60.0; + Pabb /= -60.0; +} + +compFaceIntegrals(FACE *f) +{ + double *n, w; + double k1, k2, k3, k4; + + compProjectionIntegrals(f); + + w = f->w; + n = f->norm; + k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; + + Fa = k1 * Pa; + Fb = k1 * Pb; + Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); + + Faa = k1 * Paa; + Fbb = k1 * Pbb; + Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb + + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faaa = k1 * Paaa; + Fbbb = k1 * Pbbb; + Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab + + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb + + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) + + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faab = k1 * Paab; + Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); + Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb + + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); +} + +void compVolumeIntegrals(POLYHEDRON *p) +{ + FACE *f; + double nx, ny, nz; + int i; + + T0 = T1[X] = T1[Y] = T1[Z] + = T2[X] = T2[Y] = T2[Z] + = TP[X] = TP[Y] = TP[Z] = 0; + + for (i = 0; i < p->numFaces; i++) { + + f = &p->faces[i]; + + nx = fabs(f->norm[X]); + ny = fabs(f->norm[Y]); + nz = fabs(f->norm[Z]); + if (nx > ny && nx > nz) C = X; + else C = (ny > nz) ? Y : Z; + A = (C + 1) % 3; + B = (A + 1) % 3; + + compFaceIntegrals(f); + + T0 += f->norm[X] * ((A == X) ? Fa : ((B == X) ? Fb : Fc)); + + T1[A] += f->norm[A] * Faa; + T1[B] += f->norm[B] * Fbb; + T1[C] += f->norm[C] * Fcc; + T2[A] += f->norm[A] * Faaa; + T2[B] += f->norm[B] * Fbbb; + T2[C] += f->norm[C] * Fccc; + TP[A] += f->norm[A] * Faab; + TP[B] += f->norm[B] * Fbbc; + TP[C] += f->norm[C] * Fcca; + } + + T1[X] /= 2; T1[Y] /= 2; T1[Z] /= 2; + T2[X] /= 3; T2[Y] /= 3; T2[Z] /= 3; + TP[X] /= 2; TP[Y] /= 2; TP[Z] /= 2; +} + + +/* + ============================================================================ + main + ============================================================================ +*/ + + +int main(int argc, char *argv[]) +{ + POLYHEDRON p; + double density, mass; + double r[3]; /* center of mass */ + double J[3][3]; /* inertia tensor */ + + if (argc != 2) { + printf("usage: %s \n", argv[0]); + exit(0); + } + + readPolyhedron(argv[1], &p); + + compVolumeIntegrals(&p); + + + printf("\nT1 = %+20.6f\n\n", T0); + + printf("Tx = %+20.6f\n", T1[X]); + printf("Ty = %+20.6f\n", T1[Y]); + printf("Tz = %+20.6f\n\n", T1[Z]); + + printf("Txx = %+20.6f\n", T2[X]); + printf("Tyy = %+20.6f\n", T2[Y]); + printf("Tzz = %+20.6f\n\n", T2[Z]); + + printf("Txy = %+20.6f\n", TP[X]); + printf("Tyz = %+20.6f\n", TP[Y]); + printf("Tzx = %+20.6f\n\n", TP[Z]); + + density = 1.0; /* assume unit density */ + + mass = density * T0; + + /* compute center of mass */ + r[X] = T1[X] / T0; + r[Y] = T1[Y] / T0; + r[Z] = T1[Z] / T0; + + /* compute inertia tensor */ + J[X][X] = density * (T2[Y] + T2[Z]); + J[Y][Y] = density * (T2[Z] + T2[X]); + J[Z][Z] = density * (T2[X] + T2[Y]); + J[X][Y] = J[Y][X] = - density * TP[X]; + J[Y][Z] = J[Z][Y] = - density * TP[Y]; + J[Z][X] = J[X][Z] = - density * TP[Z]; + + /* translate inertia tensor to center of mass */ + J[X][X] -= mass * (r[Y]*r[Y] + r[Z]*r[Z]); + J[Y][Y] -= mass * (r[Z]*r[Z] + r[X]*r[X]); + J[Z][Z] -= mass * (r[X]*r[X] + r[Y]*r[Y]); + J[X][Y] = J[Y][X] += mass * r[X] * r[Y]; + J[Y][Z] = J[Z][Y] += mass * r[Y] * r[Z]; + J[Z][X] = J[X][Z] += mass * r[Z] * r[X]; + + printf("center of mass: (%+12.6f,%+12.6f,%+12.6f)\n\n", r[X], r[Y], r[Z]); + + printf("inertia tensor with origin at c.o.m. :\n"); + printf("%+15.6f %+15.6f %+15.6f\n", J[X][X], J[X][Y], J[X][Z]); + printf("%+15.6f %+15.6f %+15.6f\n", J[Y][X], J[Y][Y], J[Y][Z]); + printf("%+15.6f %+15.6f %+15.6f\n\n", J[Z][X], J[Z][Y], J[Z][Z]); + +}