b98a01b28c
for six DoF motion bodies that are not principal axis aligned shapes to start with. Calculates the best match axes to give the most naturl transformation from the Cartesian axes. The eigenvectors are returned in the order relating to ascending magnitude of their eigenvalues - not necessarily in a right handed triplet.
635 lines
17 KiB
C
635 lines
17 KiB
C
/*---------------------------------------------------------------------------*\
|
|
========= |
|
|
\\ / F ield | OpenFOAM: The Open Source CFD Toolbox
|
|
\\ / O peration |
|
|
\\ / A nd | Copyright (C) 2009-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
|
|
|
|
Application
|
|
momentOfInertiaTest
|
|
|
|
Description
|
|
Calculates the inertia tensor and principal axes and moments of a
|
|
command line specified triSurface. Inertia can either be of the
|
|
solid body or of a thin shell.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
#include "argList.H"
|
|
#include "ListOps.H"
|
|
#include "face.H"
|
|
#include "tetPointRef.H"
|
|
#include "triFaceList.H"
|
|
#include "triSurface.H"
|
|
#include "OFstream.H"
|
|
#include "meshTools.H"
|
|
#include "Random.H"
|
|
#include "transform.H"
|
|
#include "IOmanip.H"
|
|
#include "Pair.H"
|
|
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
|
|
|
|
using namespace Foam;
|
|
|
|
void massPropertiesSolid
|
|
(
|
|
const pointField& pts,
|
|
const triFaceList triFaces,
|
|
scalar density,
|
|
scalar& mass,
|
|
vector& cM,
|
|
tensor& J
|
|
)
|
|
{
|
|
// Reimplemented from: Wm4PolyhedralMassProperties.cpp
|
|
// File Version: 4.10.0 (2009/11/18)
|
|
|
|
// Geometric Tools, LC
|
|
// Copyright (c) 1998-2010
|
|
// Distributed under the Boost Software License, Version 1.0.
|
|
// http://www.boost.org/LICENSE_1_0.txt
|
|
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
|
|
|
|
// Boost Software License - Version 1.0 - August 17th, 2003
|
|
|
|
// Permission is hereby granted, free of charge, to any person or
|
|
// organization obtaining a copy of the software and accompanying
|
|
// documentation covered by this license (the "Software") to use,
|
|
// reproduce, display, distribute, execute, and transmit the
|
|
// Software, and to prepare derivative works of the Software, and
|
|
// to permit third-parties to whom the Software is furnished to do
|
|
// so, all subject to the following:
|
|
|
|
// The copyright notices in the Software and this entire
|
|
// statement, including the above license grant, this restriction
|
|
// and the following disclaimer, must be included in all copies of
|
|
// the Software, in whole or in part, and all derivative works of
|
|
// the Software, unless such copies or derivative works are solely
|
|
// in the form of machine-executable object code generated by a
|
|
// source language processor.
|
|
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
|
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE AND
|
|
// NON-INFRINGEMENT. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR
|
|
// ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR
|
|
// OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
|
|
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
|
|
// USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
|
|
const scalar r6 = 1.0/6.0;
|
|
const scalar r24 = 1.0/24.0;
|
|
const scalar r60 = 1.0/60.0;
|
|
const scalar r120 = 1.0/120.0;
|
|
|
|
// order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx
|
|
scalarField integrals(10, 0.0);
|
|
|
|
forAll(triFaces, i)
|
|
{
|
|
const triFace& tri(triFaces[i]);
|
|
|
|
// vertices of triangle i
|
|
vector v0 = pts[tri[0]];
|
|
vector v1 = pts[tri[1]];
|
|
vector v2 = pts[tri[2]];
|
|
|
|
// cross product of edges
|
|
vector eA = v1 - v0;
|
|
vector eB = v2 - v0;
|
|
vector n = eA ^ eB;
|
|
|
|
// compute integral terms
|
|
scalar tmp0, tmp1, tmp2;
|
|
|
|
scalar f1x, f2x, f3x, g0x, g1x, g2x;
|
|
|
|
tmp0 = v0.x() + v1.x();
|
|
f1x = tmp0 + v2.x();
|
|
tmp1 = v0.x()*v0.x();
|
|
tmp2 = tmp1 + v1.x()*tmp0;
|
|
f2x = tmp2 + v2.x()*f1x;
|
|
f3x = v0.x()*tmp1 + v1.x()*tmp2 + v2.x()*f2x;
|
|
g0x = f2x + v0.x()*(f1x + v0.x());
|
|
g1x = f2x + v1.x()*(f1x + v1.x());
|
|
g2x = f2x + v2.x()*(f1x + v2.x());
|
|
|
|
scalar f1y, f2y, f3y, g0y, g1y, g2y;
|
|
|
|
tmp0 = v0.y() + v1.y();
|
|
f1y = tmp0 + v2.y();
|
|
tmp1 = v0.y()*v0.y();
|
|
tmp2 = tmp1 + v1.y()*tmp0;
|
|
f2y = tmp2 + v2.y()*f1y;
|
|
f3y = v0.y()*tmp1 + v1.y()*tmp2 + v2.y()*f2y;
|
|
g0y = f2y + v0.y()*(f1y + v0.y());
|
|
g1y = f2y + v1.y()*(f1y + v1.y());
|
|
g2y = f2y + v2.y()*(f1y + v2.y());
|
|
|
|
scalar f1z, f2z, f3z, g0z, g1z, g2z;
|
|
|
|
tmp0 = v0.z() + v1.z();
|
|
f1z = tmp0 + v2.z();
|
|
tmp1 = v0.z()*v0.z();
|
|
tmp2 = tmp1 + v1.z()*tmp0;
|
|
f2z = tmp2 + v2.z()*f1z;
|
|
f3z = v0.z()*tmp1 + v1.z()*tmp2 + v2.z()*f2z;
|
|
g0z = f2z + v0.z()*(f1z + v0.z());
|
|
g1z = f2z + v1.z()*(f1z + v1.z());
|
|
g2z = f2z + v2.z()*(f1z + v2.z());
|
|
|
|
// update integrals
|
|
integrals[0] += n.x()*f1x;
|
|
integrals[1] += n.x()*f2x;
|
|
integrals[2] += n.y()*f2y;
|
|
integrals[3] += n.z()*f2z;
|
|
integrals[4] += n.x()*f3x;
|
|
integrals[5] += n.y()*f3y;
|
|
integrals[6] += n.z()*f3z;
|
|
integrals[7] += n.x()*(v0.y()*g0x + v1.y()*g1x + v2.y()*g2x);
|
|
integrals[8] += n.y()*(v0.z()*g0y + v1.z()*g1y + v2.z()*g2y);
|
|
integrals[9] += n.z()*(v0.x()*g0z + v1.x()*g1z + v2.x()*g2z);
|
|
}
|
|
|
|
integrals[0] *= r6;
|
|
integrals[1] *= r24;
|
|
integrals[2] *= r24;
|
|
integrals[3] *= r24;
|
|
integrals[4] *= r60;
|
|
integrals[5] *= r60;
|
|
integrals[6] *= r60;
|
|
integrals[7] *= r120;
|
|
integrals[8] *= r120;
|
|
integrals[9] *= r120;
|
|
|
|
// mass
|
|
mass = integrals[0];
|
|
|
|
// center of mass
|
|
cM = vector(integrals[1], integrals[2], integrals[3])/mass;
|
|
|
|
// inertia relative to origin
|
|
J.xx() = integrals[5] + integrals[6];
|
|
J.xy() = -integrals[7];
|
|
J.xz() = -integrals[9];
|
|
J.yx() = J.xy();
|
|
J.yy() = integrals[4] + integrals[6];
|
|
J.yz() = -integrals[8];
|
|
J.zx() = J.xz();
|
|
J.zy() = J.yz();
|
|
J.zz() = integrals[4] + integrals[5];
|
|
|
|
// inertia relative to center of mass
|
|
J -= mass*((cM & cM)*I - cM*cM);
|
|
|
|
// Apply density
|
|
mass *= density;
|
|
J *= density;
|
|
}
|
|
|
|
|
|
void massPropertiesShell
|
|
(
|
|
const pointField& pts,
|
|
const triFaceList triFaces,
|
|
scalar density,
|
|
scalar& mass,
|
|
vector& cM,
|
|
tensor& J
|
|
)
|
|
{
|
|
// Reset properties for accumulation
|
|
|
|
mass = 0.0;
|
|
cM = vector::zero;
|
|
J = tensor::zero;
|
|
|
|
// Find centre of mass
|
|
|
|
forAll(triFaces, i)
|
|
{
|
|
const triFace& tri(triFaces[i]);
|
|
|
|
triPointRef t
|
|
(
|
|
pts[tri[0]],
|
|
pts[tri[1]],
|
|
pts[tri[2]]
|
|
);
|
|
|
|
scalar triMag = t.mag();
|
|
|
|
cM += triMag*t.centre();
|
|
|
|
mass += triMag;
|
|
}
|
|
|
|
cM /= mass;
|
|
|
|
mass *= density;
|
|
|
|
// Find inertia around centre of mass
|
|
|
|
forAll(triFaces, i)
|
|
{
|
|
const triFace& tri(triFaces[i]);
|
|
|
|
J += triPointRef
|
|
(
|
|
pts[tri[0]],
|
|
pts[tri[1]],
|
|
pts[tri[2]]
|
|
).inertia(cM, density);
|
|
}
|
|
}
|
|
|
|
|
|
tensor applyParallelAxisTheorem
|
|
(
|
|
scalar m,
|
|
const vector& cM,
|
|
const tensor& J,
|
|
const vector& refPt
|
|
)
|
|
{
|
|
// The displacement vector (refPt = cM) is the displacement of the
|
|
// new reference point from the centre of mass of the body that
|
|
// the inertia tensor applies to.
|
|
|
|
vector d = (refPt - cM);
|
|
|
|
return J + m*((d & d)*I - d*d);
|
|
}
|
|
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
argList::addNote
|
|
(
|
|
"Calculates the inertia tensor and principal axes and moments "
|
|
"of a command line specified triSurface. Inertia can either "
|
|
"be of the solid body or of a thin shell."
|
|
);
|
|
|
|
argList::noParallel();
|
|
|
|
argList::validArgs.append("surface file");
|
|
|
|
argList::addBoolOption("shellProperties");
|
|
|
|
argList::addOption
|
|
(
|
|
"density",
|
|
"scalar",
|
|
"Specify density, "
|
|
"kg/m3 for solid properties, kg/m2 for shell properties"
|
|
);
|
|
|
|
argList::addOption
|
|
(
|
|
"referencePoint",
|
|
"vector",
|
|
"Inertia relative to this point, not the centre of mass"
|
|
);
|
|
|
|
argList args(argc, argv);
|
|
|
|
fileName surfFileName(args.additionalArgs()[0]);
|
|
|
|
triSurface surf(surfFileName);
|
|
|
|
scalar density = args.optionLookupOrDefault("density", 1.0);
|
|
|
|
vector refPt = vector::zero;
|
|
|
|
bool calcAroundRefPt = args.optionReadIfPresent("referencePoint", refPt);
|
|
|
|
triFaceList faces(surf.size());
|
|
|
|
forAll(surf, i)
|
|
{
|
|
faces[i] = triFace(surf[i]);
|
|
}
|
|
|
|
scalar m = 0.0;
|
|
vector cM = vector::zero;
|
|
tensor J = tensor::zero;
|
|
|
|
if (args.optionFound("shellProperties"))
|
|
{
|
|
massPropertiesShell
|
|
(
|
|
surf.points(),
|
|
faces,
|
|
density,
|
|
m,
|
|
cM,
|
|
J
|
|
);
|
|
}
|
|
else
|
|
{
|
|
massPropertiesSolid
|
|
(
|
|
surf.points(),
|
|
faces,
|
|
density,
|
|
m,
|
|
cM,
|
|
J
|
|
);
|
|
}
|
|
|
|
if (m < 0)
|
|
{
|
|
WarningIn(args.executable() + "::main")
|
|
<< "Negative mass detected" << endl;
|
|
}
|
|
|
|
vector eVal = eigenValues(J);
|
|
|
|
tensor eVec = eigenVectors(J);
|
|
|
|
label pertI = 0;
|
|
|
|
Random rand(57373);
|
|
|
|
while ((magSqr(eVal) < VSMALL) && pertI < 10)
|
|
{
|
|
WarningIn(args.executable() + "::main")
|
|
<< "No eigenValues found, shape may have symmetry, "
|
|
<< "perturbing inertia tensor diagonal" << endl;
|
|
|
|
J.xx() *= 1.0 + SMALL*rand.scalar01();
|
|
J.yy() *= 1.0 + SMALL*rand.scalar01();
|
|
J.zz() *= 1.0 + SMALL*rand.scalar01();
|
|
|
|
eVal = eigenValues(J);
|
|
|
|
eVec = eigenVectors(J);
|
|
|
|
pertI++;
|
|
}
|
|
|
|
bool showTransform = true;
|
|
|
|
if
|
|
(
|
|
(mag(eVec.x() ^ eVec.y()) > (1.0 - SMALL))
|
|
&& (mag(eVec.y() ^ eVec.z()) > (1.0 - SMALL))
|
|
&& (mag(eVec.z() ^ eVec.x()) > (1.0 - SMALL))
|
|
)
|
|
{
|
|
// Make the eigenvectors a right handed orthogonal triplet
|
|
eVec.z() *= sign((eVec.x() ^ eVec.y()) & eVec.z());
|
|
|
|
// Finding the most natural transformation. Using Lists
|
|
// rather than tensors to allow indexed permutation.
|
|
|
|
// Cartesian basis vectors - right handed orthogonal triplet
|
|
List<vector> cartesian(3);
|
|
|
|
cartesian[0] = vector(1, 0, 0);
|
|
cartesian[1] = vector(0, 1, 0);
|
|
cartesian[2] = vector(0, 0, 1);
|
|
|
|
// Principal axis basis vectors - right handed orthogonal
|
|
// triplet
|
|
List<vector> principal(3);
|
|
|
|
principal[0] = eVec.x();
|
|
principal[1] = eVec.y();
|
|
principal[2] = eVec.z();
|
|
|
|
scalar maxMagDotProduct = -GREAT;
|
|
|
|
// Matching axis indices, first: cartesian, second:principal
|
|
|
|
Pair<label> match(-1, -1);
|
|
|
|
forAll(cartesian, cI)
|
|
{
|
|
forAll(principal, pI)
|
|
{
|
|
scalar magDotProduct = mag(cartesian[cI] & principal[pI]);
|
|
|
|
if (magDotProduct > maxMagDotProduct)
|
|
{
|
|
maxMagDotProduct = magDotProduct;
|
|
|
|
match.first() = cI;
|
|
|
|
match.second() = pI;
|
|
}
|
|
}
|
|
}
|
|
|
|
scalar sense = sign
|
|
(
|
|
cartesian[match.first()] & principal[match.second()]
|
|
);
|
|
|
|
if (sense < 0)
|
|
{
|
|
// Invert the best match direction and swap the order of
|
|
// the other two vectors
|
|
|
|
List<vector> tPrincipal = principal;
|
|
|
|
tPrincipal[match.second()] *= -1;
|
|
|
|
tPrincipal[(match.second() + 1) % 3] =
|
|
principal[(match.second() + 2) % 3];
|
|
|
|
tPrincipal[(match.second() + 2) % 3] =
|
|
principal[(match.second() + 1) % 3];
|
|
|
|
principal = tPrincipal;
|
|
|
|
vector tEVal = eVal;
|
|
|
|
tEVal[(match.second() + 1) % 3] = eVal[(match.second() + 2) % 3];
|
|
|
|
tEVal[(match.second() + 2) % 3] = eVal[(match.second() + 1) % 3];
|
|
|
|
eVal = tEVal;
|
|
}
|
|
|
|
label permutationDelta = match.second() - match.first();
|
|
|
|
if (permutationDelta != 0)
|
|
{
|
|
// Add 3 to the permutationDelta to avoid negative indices
|
|
|
|
permutationDelta += 3;
|
|
|
|
List<vector> tPrincipal = principal;
|
|
|
|
vector tEVal = eVal;
|
|
|
|
for (label i = 0; i < 3; i++)
|
|
{
|
|
tPrincipal[i] = principal[(i + permutationDelta) % 3];
|
|
|
|
tEVal[i] = eVal[(i + permutationDelta) % 3];
|
|
}
|
|
|
|
principal = tPrincipal;
|
|
|
|
eVal = tEVal;
|
|
}
|
|
|
|
label matchedAlready = match.first();
|
|
|
|
match =Pair<label>(-1, -1);
|
|
|
|
maxMagDotProduct = -GREAT;
|
|
|
|
forAll(cartesian, cI)
|
|
{
|
|
if (cI == matchedAlready)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
forAll(principal, pI)
|
|
{
|
|
if (pI == matchedAlready)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
scalar magDotProduct = mag(cartesian[cI] & principal[pI]);
|
|
|
|
if (magDotProduct > maxMagDotProduct)
|
|
{
|
|
maxMagDotProduct = magDotProduct;
|
|
|
|
match.first() = cI;
|
|
|
|
match.second() = pI;
|
|
}
|
|
}
|
|
}
|
|
|
|
sense = sign
|
|
(
|
|
cartesian[match.first()] & principal[match.second()]
|
|
);
|
|
|
|
if (sense < 0 || (match.second() - match.first()) != 0)
|
|
{
|
|
principal[match.second()] *= -1;
|
|
|
|
List<vector> tPrincipal = principal;
|
|
|
|
tPrincipal[(matchedAlready + 1) % 3] =
|
|
principal[(matchedAlready + 2) % 3]*-sense;
|
|
|
|
tPrincipal[(matchedAlready + 2) % 3] =
|
|
principal[(matchedAlready + 1) % 3]*-sense;
|
|
|
|
principal = tPrincipal;
|
|
|
|
vector tEVal = eVal;
|
|
|
|
tEVal[(matchedAlready + 1) % 3] = eVal[(matchedAlready + 2) % 3];
|
|
|
|
tEVal[(matchedAlready + 2) % 3] = eVal[(matchedAlready + 1) % 3];
|
|
|
|
eVal = tEVal;
|
|
}
|
|
|
|
eVec.x() = principal[0];
|
|
eVec.y() = principal[1];
|
|
eVec.z() = principal[2];
|
|
|
|
// {
|
|
// tensor R = rotationTensor(vector(1, 0, 0), eVec.x());
|
|
|
|
// R = rotationTensor(R & vector(0, 1, 0), eVec.y()) & R;
|
|
|
|
// Info<< "R = " << nl << R << endl;
|
|
|
|
// Info<< "R - eVec.T() " << R - eVec.T() << endl;
|
|
// }
|
|
}
|
|
else
|
|
{
|
|
WarningIn(args.executable() + "::main")
|
|
<< "Non-unique eigenvectors, cannot compute transformation "
|
|
<< "from Cartesian axes" << endl;
|
|
|
|
showTransform = false;
|
|
}
|
|
|
|
Info<< nl << setprecision(10)
|
|
<< "Density: " << density << nl
|
|
<< "Mass: " << m << nl
|
|
<< "Centre of mass: " << cM << nl
|
|
<< "Inertia tensor around centre of mass: " << nl << J << nl
|
|
<< "eigenValues (principal moments): " << eVal << nl
|
|
<< "eigenVectors (principal axes): " << nl
|
|
<< eVec.x() << nl << eVec.y() << nl << eVec.z() << endl;
|
|
|
|
if (showTransform)
|
|
{
|
|
Info<< "Transform tensor from reference state (Q). " << nl
|
|
<< "Rotation tensor required to transform "
|
|
"from the body reference frame to the global "
|
|
"reference frame, i.e.:" << nl
|
|
<< "globalVector = Q & bodyLocalVector"
|
|
<< nl << eVec.T()
|
|
<< endl;
|
|
}
|
|
|
|
if (calcAroundRefPt)
|
|
{
|
|
Info << "Inertia tensor relative to " << refPt << ": "
|
|
<< applyParallelAxisTheorem(m, cM, J, refPt)
|
|
<< endl;
|
|
}
|
|
|
|
OFstream str("axes.obj");
|
|
|
|
Info<< nl << "Writing scaled principal axes at centre of mass of "
|
|
<< surfFileName << " to " << str.name() << endl;
|
|
|
|
scalar scale = mag(cM - surf.points()[0])/eVal.component(findMin(eVal));
|
|
|
|
meshTools::writeOBJ(str, cM);
|
|
meshTools::writeOBJ(str, cM + scale*eVal.x()*eVec.x());
|
|
meshTools::writeOBJ(str, cM + scale*eVal.y()*eVec.y());
|
|
meshTools::writeOBJ(str, cM + scale*eVal.z()*eVec.z());
|
|
|
|
for (label i = 1; i < 4; i++)
|
|
{
|
|
str << "l " << 1 << ' ' << i + 1 << endl;
|
|
}
|
|
|
|
Info<< nl << "End" << nl << endl;
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// ************************************************************************* //
|