/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   \\    /   O peration     |
    \\  /    A nd           | www.openfoam.com
     \\/     M anipulation  |
-------------------------------------------------------------------------------
    Copyright (C) 2011-2016 OpenFOAM Foundation
    Copyright (C) 2015 OpenCFD Ltd.
-------------------------------------------------------------------------------
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 3 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, see <http://www.gnu.org/licenses/>.

\*---------------------------------------------------------------------------*/

#include "momentOfInertia.H"
#include "polyMeshTetDecomposition.H"

// * * * * * * * * * * * * * Static Member Functions * * * * * * * * * * * * //

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, Zero);

    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 Foam::momentOfInertia::massPropertiesShell
(
    const pointField& pts,
    const triFaceList& triFaces,
    scalar density,
    scalar& mass,
    vector& cM,
    tensor& J,
    bool doReduce
)
{
    // Reset properties for accumulation

    mass = 0.0;
    cM = Zero;
    J = 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;
    }

    if (doReduce)
    {
        reduce(cM, sumOp<vector>());
        reduce(mass, sumOp<scalar>());
    }

    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);
    }

    if (doReduce)
    {
        reduce(J, sumOp<tensor>());
    }
}


void Foam::momentOfInertia::massPropertiesSolid
(
    const triSurface& surf,
    scalar density,
    scalar& mass,
    vector& cM,
    tensor& J
)
{
    triFaceList faces(surf.size());

    forAll(surf, i)
    {
        faces[i] = triFace(surf[i]);
    }

    massPropertiesSolid(surf.points(), faces, density, mass, cM, J);
}


void Foam::momentOfInertia::massPropertiesShell
(
    const triSurface& surf,
    scalar density,
    scalar& mass,
    vector& cM,
    tensor& J,
    bool doReduce
)
{
    triFaceList faces(surf.size());

    forAll(surf, i)
    {
        faces[i] = triFace(surf[i]);
    }

    massPropertiesShell(surf.points(), faces, density, mass, cM, J, doReduce);
}


void Foam::momentOfInertia::massPropertiesPatch
(
    const polyPatch& pp,
    scalar density,
    scalar& mass,
    vector& cM,
    tensor& J,
    bool doReduce
)
{
    DynamicList<triFace> faces(3*pp.size());

    // decompose patch faces using triangle fan
    forAll(pp, faceI)
    {
        const face& f = pp[faceI];

        if (f.size() > 2)
        {
            const label v0 = 0;

            for (label i = 1; i < f.size() - 1; i++)
            {
                faces.append(triFace(f[v0], f[i],f[i + 1]));
            }
        }
    }

    triFaceList triFaces;
    triFaces.transfer(faces);
    massPropertiesShell(pp.points(), triFaces, density, mass, cM, J, doReduce);
}


Foam::tensor Foam::momentOfInertia::applyParallelAxisTheorem
(
    scalar mass,
    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 + mass*((d & d)*I - d*d);
}


Foam::tmp<Foam::tensorField> Foam::momentOfInertia::meshInertia
(
    const polyMesh& mesh
)
{
    tmp<tensorField> tTf = tmp<tensorField>(new tensorField(mesh.nCells()));

    tensorField& tf = tTf.ref();

    forAll(tf, cI)
    {
        tf[cI] = meshInertia(mesh, cI);
    }

    return tTf;
}


Foam::tensor Foam::momentOfInertia::meshInertia
(
    const polyMesh& mesh,
    label celli
)
{
    List<tetIndices> cellTets = polyMeshTetDecomposition::cellTetIndices
    (
        mesh,
        celli
    );

    triFaceList faces(cellTets.size());

    forAll(cellTets, cTI)
    {
        faces[cTI] = cellTets[cTI].faceTriIs(mesh);
    }

    scalar m = 0.0;
    vector cM = Zero;
    tensor J = Zero;

    massPropertiesSolid(mesh.points(), faces, 1.0, m, cM, J);

    return J;
}


// ************************************************************************* //
