mirror of
https://develop.openfoam.com/Development/openfoam.git
synced 2025-11-28 03:28:01 +00:00
Merge branch 'feature-rigidBody' into 'develop'
ENH: Improvement to overset that allows multiple motion solvers See merge request Development/openfoam!428
This commit is contained in:
@ -97,9 +97,15 @@ int main(int argc, char *argv[])
|
||||
dimensionedScalar("rAUf", dimTime/rho.dimensions(), 1.0)
|
||||
);
|
||||
|
||||
if (correctPhi)
|
||||
{
|
||||
#include "correctPhi.H"
|
||||
}
|
||||
#include "createUf.H"
|
||||
|
||||
#include "setCellMask.H"
|
||||
#include "setInterpolatedCells.H"
|
||||
|
||||
turbulence->validate();
|
||||
|
||||
if (!LTS)
|
||||
@ -108,9 +114,6 @@ int main(int argc, char *argv[])
|
||||
#include "setInitialDeltaT.H"
|
||||
}
|
||||
|
||||
#include "setCellMask.H"
|
||||
#include "setInterpolatedCells.H"
|
||||
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
|
||||
Info<< "\nStarting time loop\n" << endl;
|
||||
|
||||
|
||||
@ -48,7 +48,6 @@ Foam::IOobject Foam::points0MotionSolver::points0IO(const polyMesh& mesh)
|
||||
"points0",
|
||||
IOobject::READ_IF_PRESENT
|
||||
);
|
||||
|
||||
IOobject io
|
||||
(
|
||||
"points0",
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
\\ / A nd | www.openfoam.com
|
||||
\\/ M anipulation |
|
||||
-------------------------------------------------------------------------------
|
||||
Copyright (C) 2019-2020 OpenCFD Ltd.
|
||||
Copyright (C) 2019-2021 OpenCFD Ltd.
|
||||
-------------------------------------------------------------------------------
|
||||
License
|
||||
This file is part of OpenFOAM.
|
||||
@ -55,27 +55,22 @@ Foam::solidBodyMotionFunctions::drivenLinearMotion::drivenLinearMotion
|
||||
)
|
||||
:
|
||||
solidBodyMotionFunction(SBMFCoeffs, runTime),
|
||||
CofGvelocity_(SBMFCoeffs.get<word>("CofGvelocity")),
|
||||
normal_(SBMFCoeffs.getOrDefault<vector>("normal", Zero)),
|
||||
CofGvel_
|
||||
cOfGdisplacement_(SBMFCoeffs.get<word>("cOfGdisplacement")),
|
||||
CofGdisp_
|
||||
(
|
||||
IOobject
|
||||
(
|
||||
CofGvelocity_,
|
||||
cOfGdisplacement_,
|
||||
time_.timeName(),
|
||||
"uniform",
|
||||
time_,
|
||||
IOobject::READ_IF_PRESENT,
|
||||
IOobject::AUTO_WRITE
|
||||
),
|
||||
dimensionedVector(dimless, Zero)
|
||||
),
|
||||
displacement_(Zero)
|
||||
)
|
||||
{
|
||||
read(SBMFCoeffs);
|
||||
if (mag(normal_) > SMALL)
|
||||
{
|
||||
normal_ /= (mag(normal_) + SMALL);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -84,24 +79,10 @@ Foam::solidBodyMotionFunctions::drivenLinearMotion::drivenLinearMotion
|
||||
Foam::septernion
|
||||
Foam::solidBodyMotionFunctions::drivenLinearMotion::transformation() const
|
||||
{
|
||||
scalar deltaT = time_.deltaT().value();
|
||||
|
||||
vector velocity = CofGvel_.value();
|
||||
|
||||
// Take out normal component
|
||||
if (mag(normal_) > SMALL)
|
||||
{
|
||||
velocity = CofGvel_.value() - ((CofGvel_.value() & normal_)*normal_);
|
||||
}
|
||||
|
||||
DebugInFunction << "Vel on plane :" << velocity << endl;
|
||||
|
||||
// Translation of centre of gravity with constant velocity
|
||||
//const vector displacement = velocity*t;
|
||||
displacement_ += velocity*deltaT;
|
||||
|
||||
DebugInFunction << "displacement :" << CofGdisp_.value() << endl;
|
||||
quaternion R(1);
|
||||
septernion TR(septernion(-displacement_)*R);
|
||||
septernion TR(septernion(-CofGdisp_.value())*R);
|
||||
|
||||
DebugInFunction << "Time = " << time_.value()
|
||||
<< " transformation: " << TR << endl;
|
||||
|
||||
@ -63,17 +63,11 @@ class drivenLinearMotion
|
||||
{
|
||||
// Private data
|
||||
|
||||
//- Name of the meshObject to dum CofG velocity
|
||||
word CofGvelocity_;
|
||||
|
||||
//- Normal plane direction to restrict movement on a plane
|
||||
vector normal_;
|
||||
//- Name of the meshObject to dum CofG displacement
|
||||
word cOfGdisplacement_;
|
||||
|
||||
//- Uniform vector to follow
|
||||
uniformDimensionedVectorField CofGvel_;
|
||||
|
||||
//- Last displacement
|
||||
mutable vector displacement_;
|
||||
uniformDimensionedVectorField CofGdisp_;
|
||||
|
||||
|
||||
// Private Member Functions
|
||||
|
||||
@ -32,15 +32,12 @@ Description
|
||||
const cellCellStencilObject& overlap = Stencil::New(mesh);
|
||||
const labelList& cellTypes = overlap.cellTypes();
|
||||
|
||||
//label nHoles = 0;
|
||||
|
||||
cellMask.primitiveFieldRef() = 1.0;
|
||||
forAll(cellMask, cellI)
|
||||
{
|
||||
if (cellTypes[cellI] == cellCellStencil::HOLE)
|
||||
{
|
||||
cellMask[cellI] = 0.0;
|
||||
//nHoles++;
|
||||
}
|
||||
}
|
||||
cellMask.correctBoundaryConditions();
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
\\ / A nd | www.openfoam.com
|
||||
\\/ M anipulation |
|
||||
-------------------------------------------------------------------------------
|
||||
Copyright (C) 2018-2020 OpenCFD Ltd.
|
||||
Copyright (C) 2018-2021 OpenCFD Ltd.
|
||||
-------------------------------------------------------------------------------
|
||||
License
|
||||
This file is part of OpenFOAM.
|
||||
@ -61,7 +61,11 @@ Foam::RBD::restraints::prescribedRotation::prescribedRotation
|
||||
)
|
||||
:
|
||||
restraint(name, dict, model),
|
||||
omegaSet_(model_.time(), "omega")
|
||||
omegaSet_(model_.time(), "omega"),
|
||||
omega_(Zero),
|
||||
oldMom_(Zero),
|
||||
error0_(Zero),
|
||||
integral0_(Zero)
|
||||
{
|
||||
read(dict);
|
||||
}
|
||||
@ -82,7 +86,7 @@ void Foam::RBD::restraints::prescribedRotation::restrain
|
||||
const rigidBodyModelState& state
|
||||
) const
|
||||
{
|
||||
vector refDir = rotationTensor(vector(1, 0, 0), axis_) & vector(0, 1, 0);
|
||||
vector refDir = rotationTensor(vector(1, 0, 0), axis_)&vector(0, 1, 0);
|
||||
|
||||
vector oldDir = refQ_ & refDir;
|
||||
vector newDir = model_.X0(bodyID_).E() & refDir;
|
||||
@ -123,23 +127,20 @@ void Foam::RBD::restraints::prescribedRotation::restrain
|
||||
|
||||
// Adding rotation to a given body
|
||||
vector omega = model_.v(model_.master(bodyID_)).w();
|
||||
|
||||
scalar Inertia = mag(model_.I(model_.master(bodyID_)).Ic());
|
||||
|
||||
// from the definition of the angular momentum:
|
||||
// moment = Inertia*ddt(omega)
|
||||
const scalar relax = 0.5;
|
||||
|
||||
vector moment
|
||||
(
|
||||
(
|
||||
relax
|
||||
* Inertia
|
||||
* (omegaSet_.value(model_.time().value()) - omega)
|
||||
/ model_.time().deltaTValue()
|
||||
& a
|
||||
)
|
||||
* a
|
||||
);
|
||||
vector error = omegaSet_.value(model_.time().value()) - omega;
|
||||
vector integral = integral0_ + error;
|
||||
vector derivative = (error - error0_);
|
||||
|
||||
vector moment = ((p_*error + i_*integral + d_*derivative)&a)*a;
|
||||
moment *= Inertia/model_.time().deltaTValue();
|
||||
|
||||
moment = relax_*moment + (1- relax_)*oldMom_;
|
||||
|
||||
if (model_.debug)
|
||||
{
|
||||
@ -149,12 +150,20 @@ void Foam::RBD::restraints::prescribedRotation::restrain
|
||||
<< " moment " << moment << endl
|
||||
<< " oldDir " << oldDir << endl
|
||||
<< " newDir " << newDir << endl
|
||||
<< " inertia " << Inertia << endl
|
||||
<< " error " << error << endl
|
||||
<< " integral " << integral << endl
|
||||
<< " derivative " << derivative << endl
|
||||
<< " refDir " << refDir
|
||||
<< endl;
|
||||
}
|
||||
|
||||
// Accumulate the force for the restrained body
|
||||
fx[bodyIndex_] += spatialVector(moment, Zero);
|
||||
fx[bodyIndex_] += model_.X0(bodyID_).T() & spatialVector(moment, Zero);
|
||||
|
||||
oldMom_ = moment;
|
||||
error0_ = error;
|
||||
integral0_ = integral;
|
||||
}
|
||||
|
||||
|
||||
@ -180,6 +189,12 @@ bool Foam::RBD::restraints::prescribedRotation::read
|
||||
|
||||
const scalar magAxis(mag(axis_));
|
||||
|
||||
coeffs_.readEntry("relax", relax_);
|
||||
|
||||
coeffs_.readEntry("p", p_);
|
||||
coeffs_.readEntry("i", i_);
|
||||
coeffs_.readEntry("d", d_);
|
||||
|
||||
if (magAxis > VSMALL)
|
||||
{
|
||||
axis_ /= magAxis;
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
\\ / A nd | www.openfoam.com
|
||||
\\/ M anipulation |
|
||||
-------------------------------------------------------------------------------
|
||||
Copyright (C) 2018 OpenCFD Ltd.
|
||||
Copyright (C) 2021 OpenCFD Ltd.
|
||||
-------------------------------------------------------------------------------
|
||||
License
|
||||
This file is part of OpenFOAM.
|
||||
@ -44,6 +44,10 @@ Usage
|
||||
referenceOrientation | Orientation | no | I
|
||||
axis | Rotation axis (in reference) | yes |
|
||||
omega | Angular velocity (rad/s) | yes |
|
||||
relax | Relax moment with prevoius iter | yes
|
||||
p | Propoptional corrector for PDI | yes
|
||||
d | Differencial corrector for PDI | yes
|
||||
i | Integral corrector for PDI | yes
|
||||
\endtable
|
||||
|
||||
SourceFiles
|
||||
@ -85,6 +89,26 @@ class prescribedRotation
|
||||
//- Rotational velocity [rad/sec]
|
||||
TimeFunction1<vector> omegaSet_;
|
||||
|
||||
//- Cache omega
|
||||
mutable vector omega_;
|
||||
|
||||
//- Cache previous momentum
|
||||
mutable vector oldMom_;
|
||||
|
||||
//- Relax momentum
|
||||
scalar relax_;
|
||||
|
||||
//- PID constants
|
||||
|
||||
mutable vector error0_;
|
||||
|
||||
mutable vector integral0_;
|
||||
|
||||
mutable scalar p_;
|
||||
|
||||
mutable scalar i_;
|
||||
|
||||
mutable scalar d_;
|
||||
|
||||
public:
|
||||
|
||||
@ -137,7 +161,7 @@ public:
|
||||
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
|
||||
|
||||
} // End namespace RBD
|
||||
} // End namespace restraints
|
||||
} // End namespace RBD
|
||||
} // End namespace Foam
|
||||
|
||||
|
||||
@ -186,6 +186,23 @@ void Foam::RBD::rigidBodyMotion::status(const label bodyID) const
|
||||
}
|
||||
|
||||
|
||||
const Foam::vector Foam::RBD::rigidBodyMotion::vCofR(const label bodyID) const
|
||||
{
|
||||
const spatialVector velCofR(v(bodyID, Zero));
|
||||
|
||||
return velCofR.l();
|
||||
}
|
||||
|
||||
|
||||
const Foam::vector Foam::RBD::rigidBodyMotion::cCofR(const label bodyID) const
|
||||
{
|
||||
const spatialTransform CofR(X0(bodyID));
|
||||
|
||||
return CofR.r();
|
||||
}
|
||||
|
||||
|
||||
|
||||
Foam::tmp<Foam::pointField> Foam::RBD::rigidBodyMotion::transformPoints
|
||||
(
|
||||
const label bodyID,
|
||||
|
||||
@ -189,6 +189,12 @@ public:
|
||||
//- Report the status of the motion of the given body
|
||||
void status(const label bodyID) const;
|
||||
|
||||
//- Report linear velocity of the given body
|
||||
const vector vCofR(const label bodyID) const;
|
||||
|
||||
//- Report CofR of the given body
|
||||
const vector cCofR(const label bodyID) const;
|
||||
|
||||
|
||||
// Transformations
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
\\/ M anipulation |
|
||||
-------------------------------------------------------------------------------
|
||||
Copyright (C) 2016-2017 OpenFOAM Foundation
|
||||
Copyright (C) 2016-2020 OpenCFD Ltd.
|
||||
Copyright (C) 2016-2021 OpenCFD Ltd.
|
||||
-------------------------------------------------------------------------------
|
||||
License
|
||||
This file is part of OpenFOAM.
|
||||
@ -120,7 +120,12 @@ Foam::rigidBodyMeshMotion::rigidBodyMeshMotion
|
||||
rhoInf_(1.0),
|
||||
rhoName_(coeffDict().getOrDefault<word>("rho", "rho")),
|
||||
ramp_(Function1<scalar>::NewIfPresent("ramp", coeffDict())),
|
||||
curTimeIndex_(-1)
|
||||
curTimeIndex_(-1),
|
||||
cOfGdisplacement_
|
||||
(
|
||||
coeffDict().getOrDefault<word>("cOfGdisplacement", "none")
|
||||
),
|
||||
bodyIdCofG_(coeffDict().getOrDefault<label>("bodyIdCofG", -1))
|
||||
{
|
||||
if (rhoName_ == "rhoInf")
|
||||
{
|
||||
@ -208,7 +213,22 @@ Foam::rigidBodyMeshMotion::rigidBodyMeshMotion
|
||||
Foam::tmp<Foam::pointField>
|
||||
Foam::rigidBodyMeshMotion::curPoints() const
|
||||
{
|
||||
return points0() + pointDisplacement_.primitiveField();
|
||||
tmp<pointField> newPoints(points0() + pointDisplacement_.primitiveField());
|
||||
|
||||
if (moveAllCells())
|
||||
{
|
||||
return newPoints;
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp<pointField> ttransformedPts(new pointField(mesh().points()));
|
||||
pointField& transformedPts = ttransformedPts.ref();
|
||||
|
||||
UIndirectList<point>(transformedPts, pointIDs()) =
|
||||
pointField(newPoints.ref(), pointIDs());
|
||||
|
||||
return ttransformedPts;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -240,6 +260,8 @@ void Foam::rigidBodyMeshMotion::solve()
|
||||
ramp*t.lookupObject<uniformDimensionedVectorField>("g").value();
|
||||
}
|
||||
|
||||
vector oldPos = model_.cCofR(bodyIdCofG_);
|
||||
|
||||
if (test_)
|
||||
{
|
||||
const label nIter(coeffDict().get<label>("nIter"));
|
||||
@ -288,6 +310,36 @@ void Foam::rigidBodyMeshMotion::solve()
|
||||
fx
|
||||
);
|
||||
}
|
||||
|
||||
if (cOfGdisplacement_ != "none")
|
||||
{
|
||||
if (bodyIdCofG_ != -1)
|
||||
{
|
||||
if
|
||||
(
|
||||
db().time().foundObject<uniformDimensionedVectorField>
|
||||
(
|
||||
cOfGdisplacement_
|
||||
)
|
||||
)
|
||||
{
|
||||
auto& disp =
|
||||
db().time().lookupObjectRef<uniformDimensionedVectorField>
|
||||
(
|
||||
cOfGdisplacement_
|
||||
);
|
||||
|
||||
disp.value() += model_.cCofR(bodyIdCofG_) - oldPos;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
FatalErrorInFunction
|
||||
<< "CofGdisplacement is different to none." << endl
|
||||
<< "The model needs the entry body reference Id: bodyIdCofG."
|
||||
<< exit(FatalError);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (Pstream::master() && model_.report())
|
||||
@ -320,8 +372,8 @@ void Foam::rigidBodyMeshMotion::solve()
|
||||
|
||||
pointDisplacement_.primitiveFieldRef() =
|
||||
model_.transformPoints(bodyIDs, weights, points0()) - points0();
|
||||
}
|
||||
|
||||
}
|
||||
// Displacement has changed. Update boundary conditions
|
||||
pointConstraints::New
|
||||
(
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
\\/ M anipulation |
|
||||
-------------------------------------------------------------------------------
|
||||
Copyright (C) 2016-2017 OpenFOAM Foundation
|
||||
Copyright (C) 2020 OpenCFD Ltd.
|
||||
Copyright (C) 2021 OpenCFD Ltd.
|
||||
-------------------------------------------------------------------------------
|
||||
License
|
||||
This file is part of OpenFOAM.
|
||||
@ -128,6 +128,13 @@ class rigidBodyMeshMotion
|
||||
//- Current time index (used for updating)
|
||||
label curTimeIndex_;
|
||||
|
||||
//- Name of the uniformVectorField for CofG displacement
|
||||
word cOfGdisplacement_;
|
||||
|
||||
//- Body Id for the body to write CofG displacement
|
||||
label bodyIdCofG_;
|
||||
|
||||
|
||||
|
||||
// Private Member Functions
|
||||
|
||||
|
||||
@ -107,7 +107,7 @@ Foam::sixDoFRigidBodyMotionSolver::sixDoFRigidBodyMotionSolver
|
||||
dimensionedScalar(dimless, Zero)
|
||||
),
|
||||
curTimeIndex_(-1),
|
||||
CofGvelocity_(coeffDict().getOrDefault<word>("CofGvelocity", "none"))
|
||||
cOfGdisplacement_(coeffDict().getOrDefault<word>("cOfGdisplacement", "none"))
|
||||
{
|
||||
if (rhoName_ == "rhoInf")
|
||||
{
|
||||
@ -243,6 +243,8 @@ void Foam::sixDoFRigidBodyMotionSolver::solve()
|
||||
forcesDict.add("rho", rhoName_);
|
||||
forcesDict.add("CofR", motion_.centreOfRotation());
|
||||
|
||||
vector oldPos = motion_.centreOfRotation();
|
||||
|
||||
functionObjects::forces f("forces", db(), forcesDict);
|
||||
|
||||
f.calcForcesMoment();
|
||||
@ -260,22 +262,23 @@ void Foam::sixDoFRigidBodyMotionSolver::solve()
|
||||
t.deltaT0Value()
|
||||
);
|
||||
|
||||
if (CofGvelocity_ != "none")
|
||||
if (cOfGdisplacement_ != "none")
|
||||
{
|
||||
if
|
||||
(
|
||||
db().time().foundObject<uniformDimensionedVectorField>
|
||||
(
|
||||
CofGvelocity_
|
||||
cOfGdisplacement_
|
||||
)
|
||||
)
|
||||
{
|
||||
uniformDimensionedVectorField& vel =
|
||||
auto& disp =
|
||||
db().time().lookupObjectRef<uniformDimensionedVectorField>
|
||||
(
|
||||
CofGvelocity_
|
||||
cOfGdisplacement_
|
||||
);
|
||||
vel = motion_.v();
|
||||
|
||||
disp += (motion_.centreOfRotation() - oldPos);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -94,8 +94,8 @@ class sixDoFRigidBodyMotionSolver
|
||||
//- Current time index (used for updating)
|
||||
label curTimeIndex_;
|
||||
|
||||
//- Name of the uniformVectorField for CofG velocity
|
||||
word CofGvelocity_;
|
||||
//- Name of the uniformVectorField for CofG displacement
|
||||
word cOfGdisplacement_;
|
||||
|
||||
|
||||
// Private Member Functions
|
||||
|
||||
@ -26,8 +26,7 @@ solvers
|
||||
solidBodyMotionFunction drivenLinearMotion;
|
||||
|
||||
cellSet c0;
|
||||
CofGvelocity CofG;
|
||||
normal (0 0 1);
|
||||
cOfGdisplacement CofG;
|
||||
}
|
||||
|
||||
cube
|
||||
@ -42,7 +41,7 @@ solvers
|
||||
outerDistance 1001.0;
|
||||
|
||||
centreOfMass (0.5 0.5 0.4);
|
||||
CofGvelocity CofG;
|
||||
cOfGdisplacement CofG;
|
||||
|
||||
// Cuboid dimensions
|
||||
Lx 0.24;
|
||||
@ -101,7 +100,7 @@ solvers
|
||||
sixDoFRigidBodyMotionRestraint linearSpring;
|
||||
anchor (0 0.5 0.4);
|
||||
refAttachmentPt (0.5 0.5 0.4);
|
||||
stiffness 100;
|
||||
stiffness 300;
|
||||
damping 0;
|
||||
restLength 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user