zhyang-dev/openfoam
1
0
Fork 0
mirror of https://develop.openfoam.com/Development/openfoam.git synced 2025-11-28 03:28:01 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Adding moment of inertia code, unused as yet.

Browse Source
This commit is contained in:
graham
2009-12-06 12:20:38 +00:00
parent f23e3eb3ea
commit a820021a0a
8 changed files with 1079 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

403
src/meshTools/momentOfInertia/momentOfInertia.C Normal file
View File

@ -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];
}
// ************************************************************************* //

75
src/meshTools/momentOfInertia/momentOfInertia.H Normal file
View File

@ -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
// ************************************************************************* //

BIN
src/meshTools/momentOfInertia/volInt.ps.gz Normal file
View File

Binary file not shown.

146
src/meshTools/momentOfInertia/volumeIntegration/README Normal file
View File

@ -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 <polyhedron geometry filename>
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.

21
src/meshTools/momentOfInertia/volumeIntegration/cube Normal file
View File

@ -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

38
src/meshTools/momentOfInertia/volumeIntegration/icosa Normal file
View File

@ -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

16
src/meshTools/momentOfInertia/volumeIntegration/tetra Normal file
View File

@ -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

380
src/meshTools/momentOfInertia/volumeIntegration/volInt.c Normal file
View File

@ -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 <stdio.h>
#include <string.h>
#include <math.h>
/*
============================================================================
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 <polyhedron geometry filename>\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]);
}