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);
}
Esempio n. 3
0
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;
}
Esempio n. 5
0
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);
}
Esempio n. 6
0
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);
}