diff --git a/src/dynamicMesh/Make/files b/src/dynamicMesh/Make/files index c9c748ec4a..851530cd04 100644 --- a/src/dynamicMesh/Make/files +++ b/src/dynamicMesh/Make/files @@ -104,4 +104,7 @@ extrudePatchMesh/extrudePatchMesh.C polyMeshFilter/polyMeshFilterSettings.C polyMeshFilter/polyMeshFilter.C +pointPatchDist/externalPointEdgePoint.C +pointPatchDist/pointPatchDist.C + LIB = $(FOAM_LIBBIN)/libdynamicMesh diff --git a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePoint.C b/src/dynamicMesh/pointPatchDist/externalPointEdgePoint.C similarity index 95% rename from src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePoint.C rename to src/dynamicMesh/pointPatchDist/externalPointEdgePoint.C index 8ed6f62005..c3473844d6 100644 --- a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePoint.C +++ b/src/dynamicMesh/pointPatchDist/externalPointEdgePoint.C @@ -2,7 +2,7 @@ ========= | \\ / F ield | OpenFOAM: The Open Source CFD Toolbox \\ / O peration | - \\ / A nd | Copyright (C) 2013 OpenFOAM Foundation + \\ / A nd | Copyright (C) 2013-2016 OpenFOAM Foundation \\/ M anipulation | ------------------------------------------------------------------------------- License diff --git a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePoint.H b/src/dynamicMesh/pointPatchDist/externalPointEdgePoint.H similarity index 99% rename from src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePoint.H rename to src/dynamicMesh/pointPatchDist/externalPointEdgePoint.H index 85871fc5bf..cabedb0277 100644 --- a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePoint.H +++ b/src/dynamicMesh/pointPatchDist/externalPointEdgePoint.H @@ -2,7 +2,7 @@ ========= | \\ / F ield | OpenFOAM: The Open Source CFD Toolbox \\ / O peration | - \\ / A nd | Copyright (C) 2013-2015 OpenFOAM Foundation + \\ / A nd | Copyright (C) 2013-2016 OpenFOAM Foundation \\/ M anipulation | ------------------------------------------------------------------------------- License diff --git a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePointI.H b/src/dynamicMesh/pointPatchDist/externalPointEdgePointI.H similarity index 98% rename from src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePointI.H rename to src/dynamicMesh/pointPatchDist/externalPointEdgePointI.H index 44ee302e4c..5927de0776 100644 --- a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/externalPointEdgePointI.H +++ b/src/dynamicMesh/pointPatchDist/externalPointEdgePointI.H @@ -2,7 +2,7 @@ ========= | \\ / F ield | OpenFOAM: The Open Source CFD Toolbox \\ / O peration | - \\ / A nd | Copyright (C) 2013 OpenFOAM Foundation + \\ / A nd | Copyright (C) 2013-2016 OpenFOAM Foundation \\/ M anipulation | ------------------------------------------------------------------------------- License diff --git a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/pointPatchDist.C b/src/dynamicMesh/pointPatchDist/pointPatchDist.C similarity index 98% rename from src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/pointPatchDist.C rename to src/dynamicMesh/pointPatchDist/pointPatchDist.C index 7a1eb25a4e..7f2b0e54cc 100644 --- a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/pointPatchDist.C +++ b/src/dynamicMesh/pointPatchDist/pointPatchDist.C @@ -2,7 +2,7 @@ ========= | \\ / F ield | OpenFOAM: The Open Source CFD Toolbox \\ / O peration | - \\ / A nd | Copyright (C) 2013 OpenFOAM Foundation + \\ / A nd | Copyright (C) 2013-2016 OpenFOAM Foundation \\/ M anipulation | ------------------------------------------------------------------------------- License diff --git a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/pointPatchDist.H b/src/dynamicMesh/pointPatchDist/pointPatchDist.H similarity index 97% rename from src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/pointPatchDist.H rename to src/dynamicMesh/pointPatchDist/pointPatchDist.H index 7d15d30fff..6a806fa46d 100644 --- a/src/sixDoFRigidBodyMotion/sixDoFRigidBodyMotionSolver/pointPatchDist.H +++ b/src/dynamicMesh/pointPatchDist/pointPatchDist.H @@ -2,7 +2,7 @@ ========= | \\ / F ield | OpenFOAM: The Open Source CFD Toolbox \\ / O peration | - \\ / A nd | Copyright (C) 2013 OpenFOAM Foundation + \\ / A nd | Copyright (C) 2013-2016 OpenFOAM Foundation \\/ M anipulation | ------------------------------------------------------------------------------- License diff --git a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C index 9c70cfee2d..c1a086ec14 100644 --- a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C +++ b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C @@ -34,7 +34,8 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState : q_(model.nDoF(), Zero), qDot_(model.nDoF(), Zero), - qDdot_(model.nDoF(), Zero) + qDdot_(model.nDoF(), Zero), + deltaT_(0) {} @@ -46,7 +47,8 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState : q_(dict.lookupOrDefault("q", scalarField(model.nDoF(), Zero))), qDot_(dict.lookupOrDefault("qDot", scalarField(model.nDoF(), Zero))), - qDdot_(dict.lookupOrDefault("qDdot", scalarField(model.nDoF(), Zero))) + qDdot_(dict.lookupOrDefault("qDdot", scalarField(model.nDoF(), Zero))), + deltaT_(dict.lookupOrDefault("deltaT", 0)) {} diff --git a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.H b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.H index 483d98e19a..6b10ac355e 100644 --- a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.H +++ b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.H @@ -76,6 +76,9 @@ class rigidBodyModelState //- Joint acceleration scalarField qDdot_; + //- The time-step used to integrate to this state + scalar deltaT_; + public: @@ -107,6 +110,9 @@ public: //- Return access to the joint acceleration inline const scalarField& qDdot() const; + //- Return access to the time-step + inline scalar deltaT() const; + // Edit @@ -121,6 +127,9 @@ public: //- Return access to the joint acceleration inline scalarField& qDdot(); + //- Return access to the time-step + inline scalar& deltaT(); + //- Write to dictionary void write(dictionary& dict) const; diff --git a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateI.H b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateI.H index 80bb529e02..e3fb2678f4 100644 --- a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateI.H +++ b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateI.H @@ -43,6 +43,12 @@ inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::qDdot() const } +inline Foam::scalar Foam::RBD::rigidBodyModelState::deltaT() const +{ + return deltaT_; +} + + inline Foam::scalarField& Foam::RBD::rigidBodyModelState::q() { return q_; @@ -61,4 +67,10 @@ inline Foam::scalarField& Foam::RBD::rigidBodyModelState::qDdot() } +inline Foam::scalar& Foam::RBD::rigidBodyModelState::deltaT() +{ + return deltaT_; +} + + // ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateIO.C b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateIO.C index 49e9153931..bf30398b9b 100644 --- a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateIO.C +++ b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateIO.C @@ -33,6 +33,7 @@ void Foam::RBD::rigidBodyModelState::write(dictionary& dict) const dict.add("q", q_); dict.add("qDot", qDot_); dict.add("qDdot", qDdot_); + dict.add("deltaT", deltaT_); } @@ -41,6 +42,7 @@ void Foam::RBD::rigidBodyModelState::write(Ostream& os) const os.writeKeyword("q") << q_ << token::END_STATEMENT << nl; os.writeKeyword("qDot") << qDot_ << token::END_STATEMENT << nl; os.writeKeyword("qDdot") << qDdot_ << token::END_STATEMENT << nl; + os.writeKeyword("deltaT") << deltaT_ << token::END_STATEMENT << nl; } @@ -54,7 +56,8 @@ Foam::Istream& Foam::RBD::operator>> { is >> state.q_ >> state.qDot_ - >> state.qDdot_; + >> state.qDdot_ + >> state.deltaT_; // Check state of Istream is.check @@ -75,7 +78,8 @@ Foam::Ostream& Foam::RBD::operator<< { os << token::SPACE << state.q_ << token::SPACE << state.qDot_ - << token::SPACE << state.qDdot_; + << token::SPACE << state.qDdot_ + << token::SPACE << state.deltaT_; // Check state of Ostream os.check diff --git a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.C b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.C index 70b734da92..d6bf0ffbef 100644 --- a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.C +++ b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.C @@ -25,6 +25,20 @@ License #include "rigidBodyMotion.H" #include "rigidBodySolver.H" +#include "septernion.H" + +// * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * // + +void Foam::RBD::rigidBodyMotion::initialize() +{ + // Calculate the initial body-state + forwardDynamicsCorrection(rigidBodyModelState(*this)); + X00_ = X0_; + + // Update the body-state to correspond to the current joint-state + forwardDynamicsCorrection(motionState_); +} + // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * // @@ -39,7 +53,6 @@ Foam::RBD::rigidBodyMotion::rigidBodyMotion() solver_(NULL) {} - Foam::RBD::rigidBodyMotion::rigidBodyMotion ( const dictionary& dict @@ -48,11 +61,14 @@ Foam::RBD::rigidBodyMotion::rigidBodyMotion rigidBodyModel(dict), motionState_(*this), motionState0_(motionState_), + X00_(X0_.size()), aRelax_(dict.lookupOrDefault("accelerationRelaxation", 1.0)), aDamp_(dict.lookupOrDefault("accelerationDamping", 1.0)), report_(dict.lookupOrDefault("report", false)), solver_(rigidBodySolver::New(*this, dict.subDict("solver"))) -{} +{ + initialize(); +} Foam::RBD::rigidBodyMotion::rigidBodyMotion @@ -64,11 +80,14 @@ Foam::RBD::rigidBodyMotion::rigidBodyMotion rigidBodyModel(dict), motionState_(*this, stateDict), motionState0_(motionState_), + X00_(X0_.size()), aRelax_(dict.lookupOrDefault("accelerationRelaxation", 1.0)), aDamp_(dict.lookupOrDefault("accelerationDamping", 1.0)), report_(dict.lookupOrDefault("report", false)), solver_(rigidBodySolver::New(*this, dict.subDict("solver"))) -{} +{ + initialize(); +} // * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * // @@ -79,6 +98,23 @@ Foam::RBD::rigidBodyMotion::~rigidBodyMotion() // * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * // +Foam::spatialTransform Foam::RBD::rigidBodyMotion::X00 +( + const label bodyId +) const +{ + if (merged(bodyId)) + { + const subBody& mBody = mergedBody(bodyId); + return mBody.masterXT() & X00_[mBody.masterID()]; + } + else + { + return X00_[bodyId]; + } +} + + void Foam::RBD::rigidBodyMotion::solve ( scalar deltaT, @@ -86,7 +122,12 @@ void Foam::RBD::rigidBodyMotion::solve const Field& fx ) { - deltaT_ = deltaT; + motionState_.deltaT() = deltaT; + + if (motionState0_.deltaT() < SMALL) + { + motionState0_.deltaT() = deltaT; + } if (Pstream::master()) { @@ -115,64 +156,108 @@ void Foam::RBD::rigidBodyMotion::status() const */ } -/* -Foam::tmp Foam::RBD::rigidBodyMotion::transform + +Foam::tmp Foam::RBD::rigidBodyMotion::transformPoints ( + const label bodyID, + const scalarField& weight, const pointField& initialPoints ) const { - return - ( - centreOfRotation() - + (Q() & initialQ_.T() & (initialPoints - initialCentreOfRotation_)) - ); -} + // Calculate the transform from the initial state in the global frame + // to the current state in the global frame + spatialTransform X(X0(bodyID).inv() & X00(bodyID)); - -Foam::tmp Foam::RBD::rigidBodyMotion::transform -( - const pointField& initialPoints, - const scalarField& scale -) const -{ - // Calculate the transformation septerion from the initial state - septernion s - ( - centreOfRotation() - initialCentreOfRotation(), - quaternion(Q() & initialQ().T()) - ); + // Calculate the septernion equivalent of the transformation for 'slerp' + // interpolation + septernion s(X); tmp tpoints(new pointField(initialPoints)); pointField& points = tpoints.ref(); - forAll(points, pointi) + forAll(points, i) { // Move non-stationary points - if (scale[pointi] > SMALL) + if (weight[i] > SMALL) { - // Use solid-body motion where scale = 1 - if (scale[pointi] > 1 - SMALL) + // Use solid-body motion where weight = 1 + if (weight[i] > 1 - SMALL) { - points[pointi] = transform(initialPoints[pointi]); + points[i] = X.transformPoint(initialPoints[i]); } // Slerp septernion interpolation else { - septernion ss(slerp(septernion::I, s, scale[pointi])); - - points[pointi] = - initialCentreOfRotation() - + ss.transform - ( - initialPoints[pointi] - - initialCentreOfRotation() - ); + points[i] = + slerp(septernion::I, s, weight[i]) + .transformPoint(initialPoints[i]); } } } return tpoints; } -*/ + + +Foam::tmp Foam::RBD::rigidBodyMotion::transformPoints +( + const labelList& bodyIDs, + const List& weights, + const pointField& initialPoints +) const +{ + List ss(bodyIDs.size() + 1); + ss[bodyIDs.size()] = septernion::I; + + forAll(bodyIDs, bi) + { + const label bodyID = bodyIDs[bi]; + + // Calculate the transform from the initial state in the global frame + // to the current state in the global frame + spatialTransform X(X0(bodyID).inv() & X00(bodyID)); + + // Calculate the septernion equivalent of the transformation + ss[bi] = septernion(X); + } + + tmp tpoints(new pointField(initialPoints)); + pointField& points = tpoints.ref(); + + List w(ss.size()); + + forAll(points, i) + { + // Sum (1 - wi) and find the maximum wi + scalar sum1mw = 0; + scalar maxw = 0; + + forAll(bodyIDs, bi) + { + w[bi] = (*(weights[bi]))[i]; + sum1mw += 1 - w[bi]; + maxw = max(maxw, w[bi]); + } + + // Calculate the limiter for (1 - wi) to ensure the sum(wi) = maxw + scalar lambda = (w.size() - 1 - maxw)/sum1mw; + + // Limit (1 - wi) and sum the resulting wi + scalar sumw = 0; + forAll(bodyIDs, bi) + { + w[bi] = 1 - lambda*(1 - w[bi]); + sumw += w[bi]; + } + + // Calculate the weight for the stationary far-field + w[bodyIDs.size()] = 1 - sumw; + + points[i] = average(ss, w).transformPoint(initialPoints[i]); + } + + return tpoints; +} + // ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H index b2e4562b80..4e3203de59 100644 --- a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H +++ b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H @@ -79,11 +79,8 @@ class rigidBodyMotion //- Motion state data object for previous time-step rigidBodyModelState motionState0_; - //- The current time-step - scalar deltaT_; - - //- The previous time-step - scalar deltaT0_; + //- Initial transform for external forces to the bodies reference frame + List X00_; //- Acceleration relaxation coefficient scalar aRelax_; @@ -98,9 +95,17 @@ class rigidBodyMotion autoPtr solver_; - //- Construct as copy + // Private Member Functions + + //- Initialize the body-state + void initialize(); + + //- Disallow copy construct rigidBodyMotion(const rigidBodyMotion&); + //- Disallow default bitwise assignment + void operator=(const rigidBodyMotion&); + public: @@ -140,6 +145,10 @@ public: //- Return the motion state for modification inline rigidBodyModelState& state(); + //- Return the initial transform to the global frame for the + // given body + spatialTransform X00(const label bodyId) const; + // Edit @@ -164,23 +173,26 @@ public: // Transformations - /* - //- Transform the given initial state point by the current motion - // state - inline point transform(const point& initialPoints) const; - - //- Transform the given initial state pointField by the current - // motion state - tmp transform(const pointField& initialPoints) const; - - //- Transform the given initial state pointField by the current - // motion state scaled by the given scale - tmp transform + //- Transform the given initial pointField of the specified body + // to correspond to the current motion state scaled using + // 'slerp' interpolation + tmp transformPoints ( - const pointField& initialPoints, - const scalarField& scale + const label bodyID, + const scalarField& weight, + const pointField& initialPoints ) const; - */ + + //- Transform the given initial pointField of the specified body + // to correspond to the current motion state scaled using + // 'slerp' interpolation + tmp transformPoints + ( + const labelList& bodyIDs, + const List& weights, + const pointField& initialPoints + ) const; + //- Write void write(Ostream&) const; diff --git a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H index 0de439f807..8b11b1ca28 100644 --- a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H +++ b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H @@ -48,23 +48,7 @@ Foam::RBD::rigidBodyMotion::state() inline void Foam::RBD::rigidBodyMotion::newTime() { motionState0_ = motionState_; - deltaT0_ = deltaT_; } -/* -inline Foam::point Foam::RBD::rigidBodyMotion::transform -( - const point& initialPoint -) const -{ - return - ( - centreOfRotation() - + (Q() & initialQ_.T() & (initialPoint - initialCentreOfRotation_)) - ); -} -*/ - - // ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H index cdcdc0af59..1ab119bcee 100644 --- a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H +++ b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H @@ -62,15 +62,6 @@ protected: //- Return the motion state inline rigidBodyModelState& state(); - //- Return the previous motion state - inline const rigidBodyModelState& state0() const; - - //- Return the current time-step - inline scalar deltaT() const; - - //- Return the previous time-step - inline scalar deltaT0() const; - //- Return the current joint position and orientation inline scalarField& q(); @@ -82,6 +73,12 @@ protected: //- Return the current joint acceleration inline scalarField& qDdot(); + //- Return the current time-step + inline scalar deltaT() const; + + + //- Return the previous motion state + inline const rigidBodyModelState& state0() const; //- Return the current joint position and orientation inline const scalarField& q0() const; @@ -94,6 +91,9 @@ protected: //- Return the current joint acceleration inline const scalarField& qDdot0() const; + //- Return the previous time-step + inline scalar deltaT0() const; + //- Acceleration damping coefficient (for steady-state simulations) inline scalar aDamp() const; diff --git a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolverI.H b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolverI.H index 1d781854b1..53736a02ab 100644 --- a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolverI.H +++ b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolverI.H @@ -31,25 +31,6 @@ inline Foam::RBD::rigidBodyModelState& Foam::RBD::rigidBodySolver::state() } -inline const Foam::RBD::rigidBodyModelState& -Foam::RBD::rigidBodySolver::state0() const -{ - return model_.motionState0_; -} - - -inline Foam::scalar Foam::RBD::rigidBodySolver::deltaT() const -{ - return model_.deltaT_; -} - - -inline Foam::scalar Foam::RBD::rigidBodySolver::deltaT0() const -{ - return model_.deltaT0_; -} - - inline Foam::scalarField& Foam::RBD::rigidBodySolver::q() { return state().q(); @@ -68,6 +49,18 @@ inline Foam::scalarField& Foam::RBD::rigidBodySolver::qDdot() } +inline Foam::scalar Foam::RBD::rigidBodySolver::deltaT() const +{ + return model_.motionState_.deltaT(); +} + + +inline const Foam::RBD::rigidBodyModelState& +Foam::RBD::rigidBodySolver::state0() const +{ + return model_.motionState0_; +} + inline const Foam::scalarField& Foam::RBD::rigidBodySolver::q0() const { return state0().q(); @@ -86,6 +79,12 @@ inline const Foam::scalarField& Foam::RBD::rigidBodySolver::qDdot0() const } +inline Foam::scalar Foam::RBD::rigidBodySolver::deltaT0() const +{ + return model_.motionState0_.deltaT(); +} + + inline Foam::scalar Foam::RBD::rigidBodySolver::aDamp() const { return model_.aDamp_; diff --git a/src/rigidBodyMeshMotion/Make/files b/src/rigidBodyMeshMotion/Make/files new file mode 100644 index 0000000000..f7fcbc1361 --- /dev/null +++ b/src/rigidBodyMeshMotion/Make/files @@ -0,0 +1,3 @@ +rigidBodyMeshMotion.C + +LIB = $(FOAM_LIBBIN)/librigidBodyMeshMotion diff --git a/src/rigidBodyMeshMotion/Make/options b/src/rigidBodyMeshMotion/Make/options new file mode 100644 index 0000000000..92a8f72b4d --- /dev/null +++ b/src/rigidBodyMeshMotion/Make/options @@ -0,0 +1,14 @@ +EXE_INC = -ggdb3 -DFULLDEBUG \ + -I$(LIB_SRC)/rigidBodyDynamics/lnInclude \ + -I$(LIB_SRC)/finiteVolume/lnInclude \ + -I$(LIB_SRC)/meshTools/lnInclude \ + -I$(LIB_SRC)/postProcessing/functionObjects/forces/lnInclude \ + -I$(LIB_SRC)/fileFormats/lnInclude \ + -I$(LIB_SRC)/dynamicMesh/lnInclude + +LIB_LIBS = \ + -lrigidBodyDynamics \ + -lforces \ + -lmeshTools \ + -lfileFormats \ + -ldynamicMesh diff --git a/src/rigidBodyMeshMotion/rigidBodyMeshMotion.C b/src/rigidBodyMeshMotion/rigidBodyMeshMotion.C new file mode 100644 index 0000000000..a2fa567a5b --- /dev/null +++ b/src/rigidBodyMeshMotion/rigidBodyMeshMotion.C @@ -0,0 +1,357 @@ +/*---------------------------------------------------------------------------*\ + ========= | + \\ / F ield | OpenFOAM: The Open Source CFD Toolbox + \\ / O peration | + \\ / A nd | Copyright (C) 2016 OpenFOAM Foundation + \\/ 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 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 . + +\*---------------------------------------------------------------------------*/ + +#include "rigidBodyMeshMotion.H" +#include "addToRunTimeSelectionTable.H" +#include "polyMesh.H" +#include "pointPatchDist.H" +#include "pointConstraints.H" +#include "uniformDimensionedFields.H" +#include "forces.H" +#include "mathematicalConstants.H" + +// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * // + +namespace Foam +{ + defineTypeNameAndDebug(rigidBodyMeshMotion, 0); + + addToRunTimeSelectionTable + ( + motionSolver, + rigidBodyMeshMotion, + dictionary + ); +} + + +// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * // + +Foam::rigidBodyMeshMotion::bodyMesh::bodyMesh +( + const polyMesh& mesh, + const word& name, + const label bodyID, + const dictionary& dict +) +: + name_(name), + bodyID_(bodyID), + patches_(wordReList(dict.lookup("patches"))), + patchSet_(mesh.boundaryMesh().patchSet(patches_)), + di_(readScalar(dict.lookup("innerDistance"))), + do_(readScalar(dict.lookup("outerDistance"))), + weight_ + ( + IOobject + ( + name_ + ".motionScale", + mesh.time().timeName(), + mesh, + IOobject::NO_READ, + IOobject::NO_WRITE, + false + ), + pointMesh::New(mesh), + dimensionedScalar("zero", dimless, 0.0) + ) +{} + + +Foam::rigidBodyMeshMotion::rigidBodyMeshMotion +( + const polyMesh& mesh, + const IOdictionary& dict +) +: + displacementMotionSolver(mesh, dict, typeName), + model_ + ( + coeffDict(), + IOobject + ( + "rigidBodyMotionState", + mesh.time().timeName(), + "uniform", + mesh + ).headerOk() + ? IOdictionary + ( + IOobject + ( + "rigidBodyMotionState", + mesh.time().timeName(), + "uniform", + mesh, + IOobject::READ_IF_PRESENT, + IOobject::NO_WRITE, + false + ) + ) + : coeffDict() + ), + test_(coeffDict().lookupOrDefault("test", false)), + rhoInf_(1.0), + rhoName_(coeffDict().lookupOrDefault("rhoName", "rho")), + curTimeIndex_(-1) +{ + if (rhoName_ == "rhoInf") + { + rhoInf_ = readScalar(coeffDict().lookup("rhoInf")); + } + + const dictionary& bodiesDict = coeffDict().subDict("bodies"); + + forAllConstIter(IDLList, bodiesDict, iter) + { + const dictionary& bodyDict = iter().dict(); + + if (bodyDict.found("patches")) + { + bodyMeshes_.append + ( + new bodyMesh + ( + mesh, + iter().keyword(), + model_.bodyID(iter().keyword()), + bodyDict + ) + ); + } + } + + // Calculate scaling factor everywhere for each meshed body + forAll(bodyMeshes_, bi) + { + const pointMesh& pMesh = pointMesh::New(mesh); + + pointPatchDist pDist(pMesh, bodyMeshes_[bi].patchSet_, points0()); + + pointScalarField& scale = bodyMeshes_[bi].weight_; + + // Scaling: 1 up to di then linear down to 0 at do away from patches + scale.internalField() = + min + ( + max + ( + (bodyMeshes_[bi].do_ - pDist.internalField()) + /(bodyMeshes_[bi].do_ - bodyMeshes_[bi].di_), + scalar(0) + ), + scalar(1) + ); + + // Convert the scale function to a cosine + scale.internalField() = + min + ( + max + ( + 0.5 + - 0.5 + *cos(scale.internalField() + *Foam::constant::mathematical::pi), + scalar(0) + ), + scalar(1) + ); + + pointConstraints::New(pMesh).constrain(scale); + //scale.write(); + } +} + + +// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * // + +Foam::rigidBodyMeshMotion::~rigidBodyMeshMotion() +{} + + +// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * // + +Foam::tmp +Foam::rigidBodyMeshMotion::curPoints() const +{ + return points0() + pointDisplacement_.internalField(); +} + + +void Foam::rigidBodyMeshMotion::solve() +{ + const Time& t = mesh().time(); + + if (mesh().nPoints() != points0().size()) + { + FatalErrorInFunction + << "The number of points in the mesh seems to have changed." << endl + << "In constant/polyMesh there are " << points0().size() + << " points; in the current mesh there are " << mesh().nPoints() + << " points." << exit(FatalError); + } + + // Store the motion state at the beginning of the time-step + if (curTimeIndex_ != this->db().time().timeIndex()) + { + model_.newTime(); + curTimeIndex_ = this->db().time().timeIndex(); + } + + dimensionedVector g("g", dimAcceleration, vector::zero); + + if (db().foundObject("g")) + { + g = db().lookupObject("g"); + } + else if (coeffDict().found("g")) + { + coeffDict().lookup("g") >> g; + } + + model_.g() = g.value(); + + if (test_) + { + label nIter(readLabel(coeffDict().lookup("nIter"))); + + for (label i=0; i(model_.nBodies(), Zero) + ); + } + } + else + { + Field fx(model_.nBodies(), Zero); + + forAll(bodyMeshes_, bi) + { + const label bodyID = bodyMeshes_[bi].bodyID_; + + dictionary forcesDict; + + forcesDict.add("type", forces::typeName); + forcesDict.add("patches", bodyMeshes_[bi].patches_); + forcesDict.add("rhoInf", rhoInf_); + forcesDict.add("rhoName", rhoName_); + forcesDict.add("CofR", model_.X0(bodyID).r()); + + forces f("forces", db(), forcesDict); + + f.calcForcesMoment(); + + fx[bodyID].l() = f.forceEff(); + fx[bodyID].w() = f.momentEff(); + } + + model_.solve + ( + t.deltaTValue(), + scalarField(model_.nDoF(), Zero), + fx + ); + } + + + // Update the displacements + if (bodyMeshes_.size() == 1) + { + pointDisplacement_.internalField() = model_.transformPoints + ( + bodyMeshes_[0].bodyID_, + bodyMeshes_[0].weight_, + points0() + ) - points0(); + } + else + { + labelList bodyIDs(bodyMeshes_.size()); + List weights(bodyMeshes_.size()); + forAll(bodyIDs, bi) + { + bodyIDs[bi] = bodyMeshes_[bi].bodyID_; + weights[bi] = &bodyMeshes_[bi].weight_; + } + + pointDisplacement_.internalField() = + model_.transformPoints(bodyIDs, weights, points0()) - points0(); + } + + // Displacement has changed. Update boundary conditions + pointConstraints::New + ( + pointDisplacement_.mesh() + ).constrainDisplacement(pointDisplacement_); +} + + +bool Foam::rigidBodyMeshMotion::writeObject +( + IOstream::streamFormat fmt, + IOstream::versionNumber ver, + IOstream::compressionType cmp +) const +{ + IOdictionary dict + ( + IOobject + ( + "rigidBodyMotionState", + mesh().time().timeName(), + "uniform", + mesh(), + IOobject::NO_READ, + IOobject::NO_WRITE, + false + ) + ); + + model_.state().write(dict); + return dict.regIOobject::write(); +} + + +bool Foam::rigidBodyMeshMotion::read() +{ + if (displacementMotionSolver::read()) + { + model_.read(coeffDict()); + + return true; + } + else + { + return false; + } +} + + +// ************************************************************************* // diff --git a/src/rigidBodyMeshMotion/rigidBodyMeshMotion.H b/src/rigidBodyMeshMotion/rigidBodyMeshMotion.H new file mode 100644 index 0000000000..f97384e1db --- /dev/null +++ b/src/rigidBodyMeshMotion/rigidBodyMeshMotion.H @@ -0,0 +1,184 @@ +/*---------------------------------------------------------------------------*\ + ========= | + \\ / F ield | OpenFOAM: The Open Source CFD Toolbox + \\ / O peration | + \\ / A nd | Copyright (C) 2016 OpenFOAM Foundation + \\/ 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 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 . + +Class + Foam::rigidBodyMeshMotion + +Description + Rigid-body mesh motion solver for fvMesh. + + Applies septernion interpolation of movement as function of distance to the + object surface. + +SourceFiles + rigidBodyMeshMotion.C + +\*---------------------------------------------------------------------------*/ + +#ifndef rigidBodyMeshMotion_H +#define rigidBodyMeshMotion_H + +#include "displacementMotionSolver.H" +#include "rigidBodyMotion.H" + +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // + +namespace Foam +{ + +/*---------------------------------------------------------------------------*\ + Class rigidBodyMeshMotion Declaration +\*---------------------------------------------------------------------------*/ + +class rigidBodyMeshMotion +: + public displacementMotionSolver +{ + //- Class containing the patches and point motion weighting for each body + class bodyMesh + { + //- Name of the body + const word name_; + + //- ID of the body in the RBD::rigidBodyMotion + const label bodyID_; + + //- List of mesh patches associated with this body + const wordReList patches_; + + //- Patches to integrate forces + const labelHashSet patchSet_; + + //- Inner morphing distance (limit of solid-body region) + const scalar di_; + + //- Outer morphing distance (limit of linear interpolation region) + const scalar do_; + + //- Current interpolation weight + // (1 at patches and within di_, 0 at do_ and beyond) + pointScalarField weight_; + + + public: + + friend class rigidBodyMeshMotion; + + bodyMesh + ( + const polyMesh& mesh, + const word& name, + const label bodyID, + const dictionary& dict + ); + }; + + + // Private data + + //- Rigid-body model + RBD::rigidBodyMotion model_; + + //- List of the bodyMeshes containing the patches and point motion + // weighting for each body + PtrList bodyMeshes_; + + //- Switch for test-mode in which only the + // gravitational body-force is applied + Switch test_; + + //- Reference density required by the forces object for + // incompressible calculations, required if rhoName == rhoInf + scalar rhoInf_; + + //- Name of density field, optional unless used for an + // incompressible simulation, when this needs to be specified + // as rhoInf + word rhoName_; + + //- Current time index (used for updating) + label curTimeIndex_; + + + // Private Member Functions + + //- Disallow default bitwise copy construct + rigidBodyMeshMotion + ( + const rigidBodyMeshMotion& + ); + + //- Disallow default bitwise assignment + void operator=(const rigidBodyMeshMotion&); + + +public: + + //- Runtime type information + TypeName("rigidBodyMotion"); + + + // Constructors + + //- Construct from polyMesh and IOdictionary + rigidBodyMeshMotion + ( + const polyMesh&, + const IOdictionary& dict + ); + + + //- Destructor + ~rigidBodyMeshMotion(); + + + // Member Functions + + //- Return point location obtained from the current motion field + virtual tmp curPoints() const; + + //- Solve for motion + virtual void solve(); + + //- Write state using given format, version and compression + virtual bool writeObject + ( + IOstream::streamFormat fmt, + IOstream::versionNumber ver, + IOstream::compressionType cmp + ) const; + + //- Read dynamicMeshDict dictionary + virtual bool read(); +}; + + +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // + +} // End namespace Foam + +// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * // + +#endif + +// ************************************************************************* // diff --git a/src/sixDoFRigidBodyMotion/Make/files b/src/sixDoFRigidBodyMotion/Make/files index 1184c6ad72..837d2df3fd 100644 --- a/src/sixDoFRigidBodyMotion/Make/files +++ b/src/sixDoFRigidBodyMotion/Make/files @@ -28,8 +28,6 @@ pointPatchFields/derived/sixDoFRigidBodyDisplacement/sixDoFRigidBodyDisplacement pointPatchFields/derived/uncoupledSixDoFRigidBodyDisplacement/uncoupledSixDoFRigidBodyDisplacementPointPatchVectorField.C sixDoFRigidBodyMotionSolver/sixDoFRigidBodyMotionSolver.C -sixDoFRigidBodyMotionSolver/externalPointEdgePoint.C -sixDoFRigidBodyMotionSolver/pointPatchDist.C sixDoFSolvers/sixDoFSolver/sixDoFSolver.C sixDoFSolvers/sixDoFSolver/newSixDoFSolver.C