Foam::vector Foam::finiteRotation::eulerAngles(const tensor& rotT)
{
// Create a vector containing euler angles (x = roll, y = pitch, z = yaw)
vector eulerAngles;
scalar& rollAngle = eulerAngles.x();
scalar& pitchAngle = eulerAngles.y();
scalar& yawAngle = eulerAngles.z();
// Calculate roll angle
rollAngle = atan2(rotT.yz(), rotT.zz());
// Use mag to avoid negative value due to round-off
// HJ, 24/Feb/2016
// Bugfix: sqr. SS, 18/Apr/2016
const scalar c2 = sqrt(sqr(rotT.xx()) + sqr(rotT.xy()));
// Calculate pitch angle
pitchAngle = atan2(-rotT.xz(), c2);
const scalar s1 = sin(rollAngle);
const scalar c1 = cos(rollAngle);
// Calculate yaw angle
yawAngle = atan2(s1*rotT.zx() - c1*rotT.yx(), c1*rotT.yy() - s1*rotT.zy());
return eulerAngles;
}
Foam::scalar Foam::finiteRotation::rotAngle(const tensor& rotT)
{
// Alternative formulation: Daniel Schmode, 15/Feb/2009
scalar x = rotT.zy() - rotT.yz();
scalar y = rotT.xz() - rotT.zx();
scalar z = rotT.yx() - rotT.xy();
scalar r = hypot(x, hypot(y, z));
scalar t = tr(rotT);
return atan2(r, t - 1);
}
bool Foam::isoSurface::noTransform(const tensor& tt) const
{
return
(mag(tt.xx()-1) < mergeDistance_)
&& (mag(tt.yy()-1) < mergeDistance_)
&& (mag(tt.zz()-1) < mergeDistance_)
&& (mag(tt.xy()) < mergeDistance_)
&& (mag(tt.xz()) < mergeDistance_)
&& (mag(tt.yx()) < mergeDistance_)
&& (mag(tt.yz()) < mergeDistance_)
&& (mag(tt.zx()) < mergeDistance_)
&& (mag(tt.zy()) < mergeDistance_);
}
void Foam::momentOfInertia::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;
}
Foam::vector Foam::eigenValues(const tensor& t)
{
scalar i = 0;
scalar ii = 0;
scalar iii = 0;
if
(
(
mag(t.xy()) + mag(t.xz()) + mag(t.yx())
+ mag(t.yz()) + mag(t.zx()) + mag(t.zy())
)
< SMALL
)
{
// diagonal matrix
i = t.xx();
ii = t.yy();
iii = t.zz();
}
else
{
scalar a = -t.xx() - t.yy() - t.zz();
scalar b = t.xx()*t.yy() + t.xx()*t.zz() + t.yy()*t.zz()
- t.xy()*t.yx() - t.xz()*t.zx() - t.yz()*t.zy();
scalar c = - t.xx()*t.yy()*t.zz() - t.xy()*t.yz()*t.zx()
- t.xz()*t.yx()*t.zy() + t.xz()*t.yy()*t.zx()
+ t.xy()*t.yx()*t.zz() + t.xx()*t.yz()*t.zy();
// If there is a zero root
if (mag(c) < ROOTVSMALL)
{
scalar disc = sqr(a) - 4*b;
if (disc >= -SMALL)
{
scalar q = -0.5*sqrt(max(0.0, disc));
i = 0;
ii = -0.5*a + q;
iii = -0.5*a - q;
}
else
{
FatalErrorIn("eigenValues(const tensor&)")
<< "zero and complex eigenvalues in tensor: " << t
<< abort(FatalError);
}
}
else
{
scalar Q = (a*a - 3*b)/9;
scalar R = (2*a*a*a - 9*a*b + 27*c)/54;
scalar R2 = sqr(R);
scalar Q3 = pow3(Q);
// Three different real roots
if (R2 < Q3)
{
scalar sqrtQ = sqrt(Q);
scalar theta = acos(R/(Q*sqrtQ));
scalar m2SqrtQ = -2*sqrtQ;
scalar aBy3 = a/3;
i = m2SqrtQ*cos(theta/3) - aBy3;
ii = m2SqrtQ*cos((theta + twoPi)/3) - aBy3;
iii = m2SqrtQ*cos((theta - twoPi)/3) - aBy3;
}
else
{
scalar A = cbrt(R + sqrt(R2 - Q3));
// Three equal real roots
if (A < SMALL)
{
scalar root = -a/3;
return vector(root, root, root);
}
else
{
// Complex roots
WarningIn("eigenValues(const tensor&)")
<< "complex eigenvalues detected for tensor: " << t
<< endl;
return vector::zero;
}
}
}
}
// Sort the eigenvalues into ascending order
if (i > ii)
{
Swap(i, ii);
}
if (ii > iii)
{
Swap(ii, iii);
}
if (i > ii)
{
Swap(i, ii);
}
return vector(i, ii, iii);
}
Foam::vector Foam::eigenValues(const tensor& t)
{
// The eigenvalues
scalar i, ii, iii;
// diagonal matrix
if
(
(
mag(t.xy()) + mag(t.xz()) + mag(t.yx())
+ mag(t.yz()) + mag(t.zx()) + mag(t.zy())
)
< SMALL
)
{
i = t.xx();
ii = t.yy();
iii = t.zz();
}
// non-diagonal matrix
else
{
// Coefficients of the characteristic polynmial
// x^3 + a*x^2 + b*x + c = 0
scalar a =
- t.xx() - t.yy() - t.zz();
scalar b =
t.xx()*t.yy() + t.xx()*t.zz() + t.yy()*t.zz()
- t.xy()*t.yx() - t.yz()*t.zy() - t.zx()*t.xz();
scalar c =
- t.xx()*t.yy()*t.zz()
- t.xy()*t.yz()*t.zx() - t.xz()*t.zy()*t.yx()
+ t.xx()*t.yz()*t.zy() + t.yy()*t.zx()*t.xz() + t.zz()*t.xy()*t.yx();
// Auxillary variables
scalar aBy3 = a/3;
scalar P = (a*a - 3*b)/9; // == -p_wikipedia/3
scalar PPP = P*P*P;
scalar Q = (2*a*a*a - 9*a*b + 27*c)/54; // == q_wikipedia/2
scalar QQ = Q*Q;
// Three identical roots
if (mag(P) < SMALL && mag(Q) < SMALL)
{
return vector(- aBy3, - aBy3, - aBy3);
}
// Two identical roots and one distinct root
else if (mag(PPP/QQ - 1) < SMALL)
{
scalar sqrtP = sqrt(P);
scalar signQ = sign(Q);
i = ii = signQ*sqrtP - aBy3;
iii = - 2*signQ*sqrtP - aBy3;
}
// Three distinct roots
else if (PPP > QQ)
{
scalar sqrtP = sqrt(P);
scalar value = cos(acos(Q/sqrt(PPP))/3);
scalar delta = sqrt(3 - 3*value*value);
i = - 2*sqrtP*value - aBy3;
ii = sqrtP*(value + delta) - aBy3;
iii = sqrtP*(value - delta) - aBy3;
}
// One real root, two imaginary roots
// based on the above logic, PPP must be less than QQ
else
{
WarningInFunction
<< "complex eigenvalues detected for tensor: " << t
<< endl;
if (mag(P) < SMALL)
{
i = cbrt(QQ/2);
}
else
{
scalar w = cbrt(- Q - sqrt(QQ - PPP));
i = w + P/w - aBy3;
}
return vector(-VGREAT, i, VGREAT);
}
}
// Sort the eigenvalues into ascending order
if (i > ii)
{
Swap(i, ii);
}
if (ii > iii)
{
Swap(ii, iii);
}
if (i > ii)
{
Swap(i, ii);
}
return vector(i, ii, iii);
}